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ABSTRACT 


Existing  kinematics  models  for  humans  cannot  simulate  movement  beyond 
geometric  constraints.  On  the  other  hand,  complex  dynamics  models  are  computationally 
expensive  for  real  time  computer  graphics  applications  in  Virtual  Environments(VE).  To 
be  able  to  create  a  more  realistic,  real  time,  and  computationally  efficient  human  model,  a 
simple  dynamic  model  needs  to  be  developed. 

The  approach  taken  in  this  thesis  was  to  develop  a  single  rigid  body  dynamic  human 
model  with  massless  legs.  Instead  of  a  Lagrangian  model,  which  complicates  the 
calculations  exponentially  as  the  complexity  of  the  system  increases,  the  Newton-Euler 
method  was  chosen  to  derive  system  differential  equations.  Linear  state  feedback  was  used 
for  postural  control.  As  part  of  this  research,  a  previous  realistic  looking  human  model  is 
further  developed. 

The  major  conclusion  of  this  thesis  is  that  a  single  rigid  body  dynamic  model  can 
be  used  for  simulation  of  postural  control.  The  simulation  results  contained  in  this  thesis 
show  that  such  a  modeling  technique  could  be  used  to  cause  a  detailed  kinematic 
representation  of  a  human  figure  to  move  in  a  smooth  and  realistic  way  without  resorting 
to  complexity  of  a  multi-link  dynamic  model. 


VI 


TABLE  OF  CONTENTS 


l.  INTRODUCTION  . 1 

A.  MOTIVATION . 1 

B.  GOALS . 1 

C.  ORGANIZATION . 2 

n.  SURVEY  OF  PREVIOUS  WORK . 3 

A.  INTRODUCTION . 3 

B .  KINEMATIC  MODELS . 3 

C.  SINGLE  RIGID  BODY  DYNAMICS . 4 

1 .  Free  Body  Method  with  Hard  Constraints . 4 

2.  Free  Body  Method  with  Soft  Constraints . 7 

3.  Generalized  Coordinates  with  Lagrangian . 8 

D.  ARTICULATED  RIGID  BODY  DYNAMICS . 10 

E.  POSTURAL  CONTROL . 14 

F.  CONTROL  OF  STEPPING . 16 

G.  SUMMARY . 22 

m.  DETAILED  PROBLEM  STATEMENT  AND  MATHEMATICAL  FORMULA¬ 
TION  . 23 

A.  INTRODUCTION . 23 

B .  DESCRIPTION  OF  THE  MODEL . 23 

C.  THE  LAGRANGIAN  VERSION  OF  THE  PROBLEM . 25 

D.  NEWTON-EULER  FORMULATION . 30 

1 .  The  Newton-Euler  Equations  of  the  Massless  Leg . 30 

2.  The  Newton-Euler  Equations  of  the  Body . 3 1 

3 .  Combining  B  ody  and  Leg  Equations . 32 

E.  LINEARIZED  ANALYSIS  FOR  CHOOSING  GAINS . 34 

F.  SUMMARY . 41 

IV.  COMPUTER  MODELS  . 43 

vii 


A.  INTRODUCTION . 43 

B.  KINEMATIC  COMPUTER  MODEL  (DYNAMAN) . 43 

1.  Inverse  Kinematic  Equations  for  Three  Link  Planar  Manipulator . 45 

2.  Link  Descriptions  in  the  Computer  Model . 49 

3.  Stepping  Algorithms . 50 

a.  Stepping  Forward  Algorithm . 50 

b.  Stepping  Upward  Algorithm . 53 

C.  DYNAMIC  COMPUTER  MODELS . 54 

1 .  Newton- Euler  Rigid  Body  Class . 54 

2.  Numerical  Integration  Methods . 56 

3.  Dynamic  Inverted  Pendulum  Simulations . 56 

a.  A  Single  Link  Single  Rigid  Body  with  Newton-Euler  . 56 

b.  Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian . 58 

c.  Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler . 58 

D.  SUMMARY . 58 

V.  RESULTS  OF  COMPUTER  SIMULATIONS  . 59 

A.  INTRODUCTION . 59 

B.  DYNAMIC  SIMULATIONS . 59 

1.  A  Single  Link  Single  Rigid  Body  with  Newton-Euler  Method . 59 

2.  Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian  Method . 62 

3.  Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler  Method  66 

C.  RESULTS  OF  THE  DYNAMIC  SIMULATION . 70 

D.  KINEMATIC  SIMULATION  OF  STEPPING  DYNAMAN . 70 

1 .  Stepping  Forward  Algorithm . 70 

2 .  S  tepping  Upward  Algorithm . 74 

VI.  SUMMARY  AND  CONCLUSIONS  . 77 

A.  SUMMARY . 77 

B.  CONCLUSION  AND  FUTURE  RESEARCH . 78 

APPENDIX  A:  DYNAMIC  SIMULATION  SOFTWARE . 81 

viii 


APPENDIX  B:  KINEMATIC  SIMULATION  SOFTWARE . 117 

LIST  OF  REFERENCES . 159 

INITIAL  DISTRIBUTION  LIST  . 163 


IX 


X 


LIST  OF  FIGURES 


Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 

Figure 


1 :  Inverted  Pendulum . 4 

2:  Inverted  Pendulum  with  Soft  Constrains . 7 

3:  Free-body  for  Typical  Link  of  Serial  Open-chain  Planar  Articulated  Mechanism 
[KOOZ83] . 13 

4:  Single  Rigid  Body  Model  with  Variable  Length  Massless  Supporting  Legs 

[GUBI74] . 17 

5:  Stable  Gait  Function  [GUBI74] . 18 

6:  One  Legged  Hopping  Machine  with  Control  Variables  [RAEB86] . 20 

7:  Placing  The  Foot  at  The  Neutral  Point  [RABI74] . 20 


8:  Acceleration  and  Deceleration  of  the  one  Legged  Hopping  Machine  According 


to  Neutral  Point[RAIB86] . 21 

9:  Single  Rigid  Body  Model  with  A  Constant  Length  Massless  Supporting  Leg.  23 

10:  Free  Body  Diagram  for  The  Massless  Leg . 30 

1 1 :  Free  Body  Diagram  of  The  Body . 31 

12:  Kinematic  Computer  Model:  Dynaman . 43 

13:  The  Body  Parts  and  Corresponding  Dynamic  Coordinate  Systems  (DCS)  of  Dy¬ 
naman . 44 

14:  Three  Link  Planar  Manipulator . 45 

15:  Dynamic  Coordinate  System  (DCS)  Hierarchy  Tree  of  Dynaman . 50 

16:  Forward  Stepping  Algorithm . 51 

17:  Upper  Body  Rotation . 52 

18:  Change  in  the  Height  of  the  Body  During  One  Gait  Cycle . 52 

19:  The  Amount  of  Elevation  For  Stepping  Up . 53 


XI 


Figure  20:  Stepping  Up  Algorithm . 54 

Figure  21:  Inverted  Pendulum  with  Constraint  Forces . 57 

Figure  22:  Initial  Position  and  Orientation  of  the  Inverted  Pendulum . 60 

Figure  23:  Inverted  Pendulum  Moving  to  the  Upright  Orientation . 60 

Figure  24:  Inverted  Pendulum  After  Steady  State . 61 

Figure  25:  Body  Attitude  Response  of  the  Inverted  Pendulum . 61 

Figure  26:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) . 63 

Figure  27:  Two  Link  Inverted  Pendulum  Moving  to  the  Upright  Orientation  by  Control 

Torques(Lagrangian) . 63 

Figure  28:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Lagrangian) ....  64 

Figure  29:  Two  Link  Inverted  Pendulum  After  Steady  State  (Lagrangian) . 64 

Figure  30:  Body  Attitude  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) .  65 

Figure  31:  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) . 65 

Figure  32:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler) . 67 

Figure  33:  Two  Link  Pendulum  Moving  to  the  Upright  Orientation  (Newton-Euler) . 67 

Figure  34:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Newton-Euler)  68 

Figure  35:  Two  Link  Inverted  Pendulum  In  Steady  State  (Newton-Euler) . 68 

Figure  36:  Body  Attitude  Time  Response  of  the  Two  Link  Inverted  Pendulum  (Newton- 
Euler) . 69 

Figure  37:  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler)...  69 

Figure  38:  Forward  Stepping  (1) . 71 

Figure  39:  Forward  Stepping  (2) . 71 

Figure  40:  Forward  Stepping  (3) . 72 

xii 


Figure  41:  Forward  Stepping  (4) . 72 

Figure  42:  Forward  Stepping  (5) . 73 

Figure  43:  Forward  Stepping  (6) . 73 

Figure  44:  Upward  Stepping  (1) . 74 

Figure  45:  Upward  Stepping  (2) . 75 

Figure  46:  Upward  Stepping  (3) . 75 

Figure  47:  Upward  Stepping  (4) . 76 

Figure  48:  Upward  Stepping  (5) . 76 


xiii 


I 

I 


XIV 


LIST  OF  TABLES 


Table  1:  Link  Parameters  of  the  Leg  [CRAI89] 


46 


XV 


i 


XVI 


ACKNOWLEDGMENTS 


Many  thanks  to  all  whose  help  made  this  thesis  possible.  Special  thanks  to  my  two 
thesis  advisors.  To  Dr.  Michael  Zyda  I  owe  much  for  his  guidance  in  Computer  Graphics. 
My  sincerest  thanks  to  Dr.  Robert  McGhee  for  his  patience,  encouragement,  and  devotion 
to  his  students.  To  be  able  to  share  his  vast  experience  made  working  with  him  a  true 
delight.  I  would  also  like  to  thank  to  William  Frey  and  Paul  Skopowski  for  their  support. 
Finally  many  thanks  to  all  members  of  NPSNET  Research  Group  and  to  all  of  the  faculty, 
students  and  staff  of  the  Computer  Science  Department  who  helped  in  numerous  ways. 


xvm 


I.  INTRODUCTION 


A.  MOTIVATION 

There  is  an  increasing  demand  for  realistic  virtual  environments  (VE).  One  of  the 
main  branches  of  research  in  VE  is  human  motion  simulation.  Kinematics  and  dynamics 
are  the  two  disciplines  which  are  used  to  create  physically  based  mathematical  models  for 
human  motion  simulation.  Kinematic  models  are  quite  simple  to  implement  and 
computationally  inexpensive  in  comparison  to  dynamic  models.  However,  they  are  limited 
to  simulation  of  the  geometric  constraints  of  the  body  parts  of  the  human.  On  the  other 
hand,  dynamic  models  introduce  additional  physical  properties  of  the  body  parts  to  the 
simulation,  such  as  mass  and  moment  of  inertia,  which  provides  more  realism.  It  is  possible 
to  simulate  human  motion  realistically  with  detailed  dynamic  models.  However,  the  cost  of 
this  realism  is  a  high  degree  of  computational  complexity.  When  more  detailed  models  are 
chosen,  the  response  time  of  the  simulated  human  model  increases.  On  the  other  hand, 
reducing  latency  to  a  minimum  amount  of  time  is  a  important  requirement  for  virtual 
environments  in  which  humans  can  interact.  This  places  limits  on  the  complexity  of  the 
model  which  can  be  used  in  a  particular  situation. 

B.  GOALS 

The  purpose  of  this  thesis  is  to  develop  a  single  rigid  body  dynamic  human  model 
with  massless  legs  to  illustrate  that  such  a  model  can  simulate  human  motion  more 
smoothly  and  more  realistically  than  kinematic  models,  and  without  resorting  to  the 
complexity  of  multi-link  dynamic  models.  The  Newton-Euler  method  is  chosen  instead  of 
the  Lagrangian  method,  since  the  complexity  of  the  latter  grows  exponentially  when  the 
degrees  of  freedom  increase. 
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C.  ORGANIZATION 


Chapter  II  of  this  thesis  provides  background  information  and  reviews  previous 
work  relating  to  the  area  of  kinematic  and  dynamic  methods  to  simulate  human  motion.  In 
this  chapter,  articulated  rigid  body  dynamics,  postural  control,  and  control  of  stepping  are 
also  investigated  in  addition  to  single  rigid  body  dynamics.  Chapter  III  provides  an 
overview  of  the  problem  statement  for  this  thesis  and  discusses  mathematical  modeling  of 
postural  control  of  a  single  rigid  body  with  an  attached  massless  leg.  Chapter  IV  introduces 
the  developed  kinematic  and  dynamic  computer  models.  Chapter  V  presents  results  and 
conclusions  about  the  work  completed.  The  last  chapter.  Chapter  VI,  discusses 
recommendations  for  future  enhancements  and  further  research. 
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II.  SURVEY  OF  PREVIOUS  WORK 


A.  INTRODUCTION 

Mathematical  models  which  define  human  motion  in  detail  are  complex  and  quite 
nonlinear.  This  provides  a  motivation  to  start  with  simple  models  at  a  high  level  of 
abstraction.  Models  which  ignore  the  dynamic  properties  of  body  parts  are  called  kinematic 
models.  There  are  also  models  which  consider  only  the  torso  to  have  dynamic  properties. 
More  complex  models  introduce  mass  and  moment  of  inertia  for  more  than  one  body  part. 
The  more  dynamic  properties  are  added,  the  higher  becomes  the  complexity  and  non¬ 
linearity  of  the  resulting  model. 

B.  KINEMATIC  MODELS 

Kinematics  is  concerned  with  motion  without  considering  the  forces  and  torques 
that  cause  it.  It  establishes  relations  between  position,  velocity,  acceleration,  and  higher 
order  derivatives  of  position  variables. 

The  human  body  can  be  thought  as  a  set  of  connected  body  parts  (links).  Each  body 
part  (link)  has  four  link  parameters  to  be  defined:  link  length,  (a),  link  twist  (a),  link 

offset(d)  and,  joint  angle(0).  There  exists  a  coordinate  frame  attached  to  each  link 
[CRAI89], 

Manipulator  kinematics  investigates  the  transformation  from  frame  to  frame  as  the 
body  articulates.  Forward  kinematics  show  how  to  compute  the  position  and  orientation  of 
the  end-effector  according  to  link  parameters.  On  the  other  hand,  inverse  kinematics  solves 
for  link  parameters  when  the  position  and  orientation  of  the  end-effector  is  given. 

For  the  human  body,  it  can  be  assumed  that  the  joint  angle  is  the  only  variable  link 
parameter.  Forward  kinematics  can  be  used  to  solve  for  the  position  and  orientation  of  each 
body  part  as  joint  angles  are  given.  However,  this  method  is  difficult  for  the  animator.  The 
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position  and  orientation  of  each  body  part  can  also  be  computed  with  the  position  and 
orientation  of  the  end-effector  as  an  input  by  using  inverse  kinematics.  Even  though  the 
second  method  introduces  more  complexity,  in  this  approach  the  animator  is  able  to  define 
the  path  for  the  end-effector. 

C.  SINGLE  RIGID  BODY  DYNAMICS 

An  advanced  approach  to  human  motion  simulation  introduces  dynamics.  There  are 
three  most  often  used  methods:  free  body  method  with  hard  constraints,  free  body  method 
with  soft  constraints,  and  generalized  coordinates  with  Lagrangian[MCGH79]. 

1.  Free  Body  Method  with  Hard  Constraints 

This  method  looks  at  each  body  part  as  a  separate  free  element,  under  the  influences 
of  joint  torques,  gravity,  and  reaction  forces  and  moments.  The  simplest  possible  dynamical 
model  for  human  motion  is  an  inverted  pendulum  which  idealizes  the  entire  body  as  a 
single  rigid  link  with  upright  posture  maintained  by  ankle  torque. 


Figure  1 :  Inverted  Pendulum 
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In  human  motion,  joint  torques  are  created  by  muscular  contraction.  The  origin  of 


reaction  forces  and  moments  is  forces  from  other  limbs  or  the  ground.  The  force  and  torque 

equations  for  the  inverted  pendulum  are  [MCGH79] 

mx  =  F  x 

(eq.  2.1) 

my  =  Fy-mg 

(eq.  2.2) 

70  =  F  /sinG-77  ZcosG  +  M 

x  y 

(eq.  2.3) 

where  Z  is  the  distance  from  the  coordinate  origin  to  the  center  of  gravity,  I  is  moment  of 

inertia  about  the  center  of  gravity,  F,F  are  ground  reaction  forces,  and  M  is  the  joint 

x  y 

control  torque  which  is  computed  by  some  control  law 

M  =  u(x,  t ) .  (eq.  2.4) 

In  this  equation,  x  is  the  system  state  vector  and  t  is  time. 

After  expressing  reaction  forces  and  moments,  constraint  equations  are  derived 
according  to  the  geometry  of  the  system,  and  differentiated  twice.  Constraint  equations  for 
this  example  are 


x  =  /  cosG 

(eq.  2.5) 

and 

y  =  ZsinG 

(eq.  2.6) 

with  second  derivatives 

.2 

x  =  -0ZsinG-0  ZcosG 

(eq.  2.7) 

The  five  equations  above  can  be  solved  numerically  or  analytically  to  determine  the 


unknowns:  if,  y,  0 ,  F  ,  F  .  The  matrix  form  of  the  equations  is 

*  y 


m  0  0  -1  0 

0  m  0  0  -1 

0  0  /  -/sinO  /cos 

1  0  /sin0  0  0 

0  1  -Ic  osO  0  0 


-1 

X 

0 

y 

-mg 

0 

~ 

M 

-02/cos0 

- 

Fl 

-0  /sin0 

(eq.  2.9) 


The  behavior  of  the  pendulum  can  also  be  expressed  with  a  3x3  matrix 
multiplication  by  analytically  eliminating  x  and  y.  The  3x3  matrix  equation  is 
[MCGH79b] 


I  -/  sin0  /  cos 
m/sin0  1  0 

-mlc  os0  0  1 


)1 

0 

F 

= 

X 

F 

L  yj 

M 

.2 

-m0  /cos0 


-  m0  /sinO  +  mg 


(eq.  2.10) 


Since  this  system  has  only  a  single  degree  of  freedom,  a  suitable  state  vector  is 

:n 


'0\ 


x  = 


0. 


(eq.  2.11) 


so 


X  = 


"0^ 

,0 


KXV 


(eq.  2.12) 


For  simulating  this  model,  numerical  integration  can  be  used  to  compute  x(t )  for 
any  given  x(0) .  The  value  of  0  (or  x ^ )  is  calculated  by  the  system  dynamic  equations.  It 
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has  been  found  in  the  work  of  this  thesis  that  the  3x3  matrix  solution  is  approximately  two 
times  faster  than  the  5x5  one. 

2.  Free  Body  Method  with  Soft  Constraints 

A  more  realistic  approach  is  to  consider  the  connection  between  limb  segments  as 
not  rigid.  In  the  soft  constraint  method,  the  constraint  equations  are  replaced  with  functions 
defined  by  joint  variables  and  their  derivatives,  which  are  used  to  compute  constraint  forces 
with  some  gain  values.  This  increases  the  order  of  state  equation  by  a  factor  of  three  in  the 
planar  case,  and  by  a  factor  of  six  in  the  three  dimensional  case,  but  avoids  matrix 
inversion. 

The  previous  inverted  pendulum  model  becomes  as  shown  in  Figure  2  for  the  soft 
constraint  method. 


Figure  2:  Inverted  Pendulum  with  Soft  Constrains 

Instead  of  Eq.  2.5  and  Eq.  2.6,  the  soft  constraint  equations  are[MCGH79] 

x  =  xb  + loose  (eq.  2.13) 

y  =  yb  +  i  sine  (eq.2.i4) 

with  the  corresponding  soft  constraint  forces 
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Fx  =  -  kxxb  -  kxH 


(eq.  2.15) 


Fy  =  ~kyyb~kpb  (eq.2.16) 

The  state  vector  for  the  numerical  solution  is 

X  =  (0,  0,  Xb,  Xb,  yb,  yb)T  (eq-  2.17) 

After  obtaining  the  constraint  forces  by  using  the  state  vector  values,  Eq  2.1,  Eq  2.2,  and 
Eq  2.3  can  be  solved  directly  for  translational  and  angular  accelerations  instead  of  requiring 
matrix  inversion. 

3.  Generalized  Coordinates  with  Lagrangian 

Another  way  to  derive  the  system  equations  is  to  use  the  Lagrangian  method.  This 
method  does  not  compute  constraint  forces  and  moments,  but  instead  uses  the  difference  of 
kinetic  and  potential  energies  expressed  in  terms  of  generalized  coordinates. 

The  virtual  work  function,  for  any  generalized  coordinate,  0 ,  is 

6W  =  £>e§0  (eq.  2.18) 

The  Lagrangian  function  is 

L  =  K-V  (eq.  2.19) 

where  K  is  the  kinetic  energy  function  and  V  is  the  potential  energy  function.  The 
differential  equations  of  the  system  are  obtained  from  [MCGH79b] 

_dL_  _  q  (eq.  2.20) 

dtdqt  d(bi  qi 

where  represents  the  generalized  coordinate  for  each  i  and  Q ^  is  the  coefficient  of 
5 in  the  virtual  work  function. 
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Considering  the  hard  constraint  example,  Figure  1 ,  the  virtual  work  function  is 
5W  =  M50  (eq.  2.21) 

where  0  is  the  generalized  coordinate.  The  kinetic  energy  of  this  system  is 


_  1,  .2  .2  .a2 
T  =  -(mi  +my  +70  ) 


(eq.  2.22) 


or 


T  =  ^(m/20  sin0  +  m/202cos0  +  702) 

which  simplifies  to 

1  2  -2 

T  =  i(ml  +I)Q 

The  potential  energy  of  the  system  is 
V  -  mgl sin0 
Thus,  the  Lagrangian  function  is 

L  =  ^(m/2  +/)02  -mg/sin0 


(eq.  2.23) 


(eq.  2.24) 


(eq.  2.25) 


(eq.  2.26) 


By  evaluation  the  Lagrangian  derivatives,  the  result  is  that  the  angular  acceleration  of  the 
pendulum  is  given  by 

x  M-mslc  os0 

0  =  - 2—5 -  (eq.  2.27) 

I  +  ml 

For  a  numerical  solution,  the  state  vector  is 


x  = 


(eq.  2.28) 
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The  Lagrangian  approach  is  perhaps  the  most  effective  one  for  simple  cases. 
However,  with  an  increase  in  the  number  of  degrees  of  freedom  of  the  system  the 
complexity  of  the  calculations  grows  exponentially. 


D.  ARTICULATED  RIGID  BODY  DYNAMICS 

It  is  certainly  impossible  to  model  the  human  body  with  complete  realism  as  a  single 
mass  rigid  body.  One  can,  however,  abstract  it  as  a  system  of  rigid  bodies  connected 
together  with  rotary  joints  with  control  torques  acting  at  each  joint.  If  the  motion  of  each 
limb  segment  is  given,  and  the  forces  and  torques  are  to  be  computed,  this  problem  is  called 
the  inverse  dynamics  problem.  If  the  torques  are  given  and  the  accelerations  are  to  be 
determined,  this  is  called  the  direct  dynamics  problem  [KOOZ83]. 

There  is  an  algorithm,  called  Articulated  Body(AB),  for  direct  dynamics  which 
contains  three  0(  n)  recursions  [MCMI95].  In  the  first  step  of  this  method,  which  is 
Forward  Kinematics,  velocity  and  velocity  dependent  terms  are  computed  from  the  base  to 
the  tip  of  each  serial  chain  of  links.  The  orientation  of  i ’s  coordinate  system  with  respect 

to  i  -  1  ’s  specified  by  the  rotation  matrix  [CRAI89],  lR^  _  ^ ,  can  be  determined  by  using 


joint  position,  q-,  which  is  an  element  of  state  vector.  The  position  of  i ’s  coordinate 


system  origin  according  to  i  —  1 ,  is  a  constant.  Then  the  spatial  transformation 

matrix  is  [CRAI89] 


/  +  1 


X,.= 


' + lRi 

i  i  +  1  n 

it+ix  Ri 


0 

/  +  1 


R 


(eq.  2.29) 
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The  spatial  transformation  matrices,  along  with  q^are  used  to  compute  velocity 


values  of  the  links  by  using  the  relation 


ivT 

V;  =  V-_, 


(eq.  2.30) 


The  velocity-dependent  bias  forces,  (3^,  and  the  vector  of  Coriolis  and  centripetal 

accelerations,  £  ■ ,  of  each  link  are  also  determined  from  the  base  to  the  tip.  Details  of  this 
calculation  can  be  found  in  [MCMI95b]. 

In  the  second  step,  which  is  backwards  dynamics,  /t  •  and  (3t;  are  computed  from 


the  tip  to  the  base.  The  matrix  / 1  •  is  the  6X6  inertia  of  links  i  though  N .  In  other  words, 

“the  inertia  ‘felt’  at  the  i  coordinate  system  when  the  joints  from  i  +  1  to  N  are  free  to 
move”[MCMI95].  The  vector  is  the  bias  force  exerted  on  ith  link,  including  all 
outboard  torques. 

In  the  final  step,  joint  and  link  accelerations  are  computed  from  the  base  to  the  tip 
as  <2q  =  0  is  given  and  by  using  the  equations 


a'i  =  (eq.  2.31) 

and 

/;  =  flp'i  -  pt;  (eq.  2.32) 

where  f  ■  is  the  spatial  force  exerted  onto  link  i  by  its  inboard  link  which  contains  the  effect 
of  input  torque,  T-,  and  where  a'  is  a  six  element  vector  containing  the  angular 
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acceleration,  CG-,  and  translational  acceleration  vectors.  Each  <b  .  is  a  six  element  unit 
£  £ 

vector  which  specifies  the  corresponding  joint  axis. 

Another  numerical  approach  to  the  recursive  direct  dynamics  is  based  on  solving 

the  inverse  dynamics  problem  for  a  sequence  of  n  +  1  specialized  acceleration  vectors.  In 

general,  the  inverse  dynamic  equation  can  be  written  as  [KOOZ83] 

T  =  CQ-D  (eq.  2.33) 

where  the  matrix  C  is  given  by 

C  =  B~lJ  =  [C1C2C3...]  (eq.  2.34) 

and  where  each  element  of  vector  C  is  a  column  vector.  Usually,  C  is  not  known 
explicitly.  However  it  can  be  computed  with  a  numerical  approach  using  an  inverse 

T 

dynamics  model  such  as  that  in  [CRAI89].  Specifically,  for  the  case  0  =  (0,  0,  ...)  , 
from  Eq  2.33 

Tq  =  -D  (eq.  2.35) 

The  torque  Tq  can  be  computed  by  using  the  kinematic  and  dynamic  equations  of  the 

articulated  mechanism.  As  an  example,  the  model  given  in  [KOOZ83]  which  is  illustrated 
in  Figure  3  can  be  used. 

The  kinematic  equations  of  the  planar  model  in  Figure  3  are 

.2 

*'f  =  xt _  J-(Z. _  I  -  dt_  jKO/-  1  sine •  _!  +  ©/_! COS0 •  _  j ) 

.  2 

-£^(0jSin6{-  +  0j  cos@p  (eq.  2.36) 

and 
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-2 


y)  =  j'J_i  +  (^_i-^_i)(0I-icose/_1-eI_1sineI._1) 


.2 


+  ^.(Gj'CosG—  0j  cos0  ) 


y,- 1 


(eq.  2.37) 


Figure  3:  Free-body  for  Typical  Link  of  Serial  Open-chain  Planar  Articulated  Mechanism 

[KOOZ83] 

Eq.  2.36  and  Eq.  2.37  are  used  to  calculate  the  translational  accelerations  of  the 
links  from  the  base  to  the  tip.  By  using  these  accelerations,  joint  torques  can  be  computed 
in  the  same  recursive  manner,  but  from  out  to  inward  with  the  following  equations: 

Fx,  =  Fxitl  +  miXi  2-38) 

Fy,  =  Fylt ,  +  mC>i  +  mi&  («!•  2-39) 

Ti  =  Ti+rFxul(li~d>)Sia6i+Fyl.Sli~di)COs6i  <eq2'40> 

-F  djSimQj  +  FydicosQi  +  Jfit  (eq.  2.41) 
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For  the  case  0  =  (1,0,...)  ,  all  accelerations  except  0  j  would  be  equal  to  zero 
which  means 

Tx  -  Cx-D  (eq.  2.42) 

The  joint  torque,  T  j ,  can  be  calculated  with  the  recursive  method,  which  is  described 
above.  Then  C  j  can  be  calculated  as 

C{  =  Tx+  D  =  Tl~TQ  (eq.  2.43) 

With  the  same  logic  C-  is 

Ci=Ti-T0  (eq.  2.44) 

Thus,  the  C  matrix  can  be  computed  numerically  by  following  these  steps  n  times  for  a  n- 
link  system.  Then  0  is  given  by 

0  =  C~\T-Tq )  (eq.  2.45) 

where  T  is  any  arbitrary  torque  and  Tq  is  the  “equilibrium”  torque  defined  by  Eq  2.35. 

3 

Because  of  the  required  matrix  inversion,  the  second  method  has  0(n  )  complexity.  It  has 

3 

been  shown  [MCMI94],  that  0(n  )  methods  are  best  for  simple  serial  chains  of  rigid 
bodies  when  n  <  3 ,  and  0(n )  methods  are  better  for  n  >  3  . 

E.  POSTURAL  CONTROL 

It  is  clearly  necessary  to  begin  with  modeling  stable  standing  of  a  human  before 
considering  control  of  walking.  All  the  methods  derived  in  the  previous  sections  show  how 
to  compute  joint  angle  accelerations  according  to  joint  control  torques.  The  question  is  how 
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to  formulate  control  torques.  One  straightforward  way  is  linear  state  feedback  control 
[MCGH86], 


A  suitable  state  vector  for  postural  control  can  be  defined  as 

x  = 

where  0  is  an  nxl  vector  of  joint  positions  and  0  is  the  nxl  vector  of  joint  velocities.  The 
corresponding  body  state  equation  is[KOOZ83] 

i  =  f(x)  +  E{x)  T  (eq.  2.47) 

In  Eq.  2.10  /  and,  E  are  in  general  not  available  analytically.  However,  with  the 
numerical  approach,  explained  in  the  previous  section,  the  same  equation  can  be  expressed 
as 

( T-Tq )  (eq.  2.48) 

where  I  is  the  unit  matrix. 

Linear  feedback  systems  permit  much  flexibility  to  the  designer,  such  as  pole 

assignment  [KOOZ83].  Since  C  and  Tq  are  functions  of  X,  the  system  is  not  linear. 
However,  it  can  be  linearized  around  0  for  joint  angular  rates  and  90  degrees  for  joint  angles 
for  erect  body  position.  Then  the  linearized  body  state  equation  can  be  written  as 

x  =  Fx+  GT  (eq.  2.49) 

where  F  is  the  linearized  system  matrix  and  G  is  the  control  distribution  matrix 
[KOOZ83], 

Under  the  assumption  of  linear  state  feedback,  the  control  torques  vector  will  be 


x  = 


0  / 
0  0 


x  + 


0 


c 


r-l 


0 


0 


(eq.  2.46) 
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T  =  Kx 


(eq.  2.50) 


where  K  is  the  gain  matrix.  If  Eq.  2.12  and  Eq.  2.13  are  combined,  the  derivative  of  the 
state  vector  is  then 

x  =  Fx  +  GKx  =  [F  +  GK]x  =  Hx  (eq.  2.51) 

Gain  values  in  K  should  be  chosen  so  that  all  eigenvalues  of  H  have  negative  real  parts  in 
order  to  obtain  a  stable  postural  control  [KOOZ83]  [CAMA77]. 

F.  CONTROL  OF  STEPPING 

If  all  the  postural  control  system  eigenvalues  have  negative  real  parts,  the 
articulated  mechanism  can  maintain  its  upright  standing  posture.  The  next  step  is  to  add 
stepping  to  the  system.  At  this  point  there  are  two  options  to  choose:  static  or  dynamic 
balancing.  “A  statically  balanced  system  avoids  tipping  and  ensuing  horizontal 
accelerations  by  keeping  the  center  of  mass  of  the  body  over  the  polygon  of  the  support, 
formed  by  the  feet.”  [RABI86].  On  the  other  hand,  a  dynamically  balanced  system  can 
depart  from  static  equilibrium  and  is  permitted  to  tip  and  accelerate  for  short  period  of  time. 
By  observing  human  walking,  one  can  easily  say  that  dynamical  balancing  needs  to  be 
chosen  for  simulating  a  stepping  human. 

A  step  length  control  method  with  dynamic  stability  was  presented  by  Gubina 
[GUBI74],  Gubina’s  model  has  a  single  rigid  body  with  two  supporting  massless  legs.  The 
system  has  three  degrees  of  freedom.  The  state  vector  for  the  linearized  system  is 

x  =  (r-r0,  r,  0J,  01,02,  ©2)  (eq.  2.52) 

where  0  j  is  the  leg  angle,  ©2  is  the  body  attitude,  r  is  the  leg  length,  and  is  the 
desired  leg  length. 
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Figure  4:  Single  Rigid  Body  Model  with  Variable  Length  Massless  Supporting  Legs 

[GUBI74] 

The  input  vector  for  the  linear  state  equations  is 
T 

u  =  {u^u^)  (eq.  2.53) 

where 

=  F-Fq  (eq.  2.54) 

and 

«2  =  M  —  Mq  (eq.  2.55) 

In  these  equations,  F  is  the  control  force  applied  along  the  leg,  M  is  the  control  torque 
applied  at  the  hip  and  Fq,  Mq  are  bias  force  and  torque  respectively. 

The  input  vector,  u ,  is  used  to  control  leg  length,  and  body  attitude  and  step  control 
are  used  to  produce  the  desired  forward  motion.  The  leg  length  is  controlled  by  U  j  as 
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u j  =  A  j  (r  —  rQ)  +  h^r 


(eq.  2.56) 


where  h  j  and  are  gain  constants.  The  body  attitude  is  controlled  by  as 

u2  =  ^5(^2  —  a)  +  ^6^2  (eq.  2.57) 

where  h ^  and  are  gain  constants  and  OC  is  the  bias  term  to  permit  the  desired  attitude. 

When  the  initial  conditions  are  assumed  as  r(0)  =  t*q,  r(0)  =  0, 
©2(0)  =  0 ,  02(0)  =  0  >  the  leg  angle  is  governed  by  the  linearized  system  equation 

=  0  (eq.  2.58) 

where 


Figure  5:  Stable  Gait  Function  [GUBI74] 

As  seen  in  Figure  5,  the  leg  has  a  negative  angular  velocity  growth  rate  for  negative 
angles  and  positive  angular  velocity  growth  rate  for  positive  angles.  The  horizontal  dotted 
line  which  is  extended  from  the  right  to  the  left  represents  the  change  of  supporting  leg.  In 
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the  absence  of  disturbances,  the  leg  angle  can  be  described  with  aperiodic  function,  9  j  (f  ) . 

The  existence  of  such  a  function  provides  a  “stable  gait”.  However,  disturbances  do  occur 
in  locomotion  systems.  That  is  why  feedback  control  is  needed.  With  such  a  control 
mechanism,  the  leg  angle  differential  equation  becomes 


9  1 

x  - 

1 

b1  9 

s 

9 

us(k)b(t-kT) 


(eq.  2.60) 


where  8  denotes  the  unit  inpulse  function  and 

T 

xs  =  (*3’x4)  (eq.  2.61) 

It  is  shown  in  [GUBI74]  that  a  suitable  discrete  time  control  vector  for  this  system  is 


us(k)  =  h3[x3(lcT)  -Qs]+  h4 


v 


0 


x4(kT)  -  - 


0 


+  9 


0 


(eq.  2.62) 


The  angle,  9  ^ ,  is  the  desired  total  leg  angle  excursion  over  one  cycle,  while  Vq  is  the 
desired  forward  speed. 

Another  foot  placement  algorithm  for  controlling  forward  speed  is  investigated  by 
Raibert[RABI74],  In  this  work,  a  one  legged  hopping  machine  was  developed  to 
investigate  dynamically  balanced  running  robots.  The  machine  has  two  main  parts:  the 
body  and  the  leg  which  is  connected  to  the  body  with  a  hinge.  There  also  exists  a  hip 
actuator  to  be  able  to  apply  a  torque  from  the  body  to  the  leg. 
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Body 


Figure  6:  One  Legged  Hopping  Machine  with  Control  Variables  [RAIB86] 


Figure  7:  Placing  The  Foot  at  The  Neutral  Point  [RABI74] 

For  each  forward  speed  there  is  a  foot  position  which  causes  zero  acceleration  in  the 
direction  of  motion.  This  point  is  called  the  “neutral  point”.  Placing  the  foot  behind  the 
neutral  point  accelerates  the  machine,  placing  the  foot  in  front  of  the  neutral  point 
decelerates  it. 
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Acceleration 


Deceleration 


Neutral  Point 


Neutral  Point 


Figure  8:  Acceleration  and  Deceleration  of  the  one  Legged  Hopping  Machine  According 

to  Neutral  Point[RAIB86] 

To  place  the  foot  at  the  neutral  point,  the  control  system  should  extend  the  leg  such 
that  [RAIB86] 


x 


f  0 


(eq.  2.63) 


where  x  f  is  the  forward  displacement  of  the  foot  with  respect  to  the  center  mass,  x.  is 
Jo 

the  forward  speed,  and  T  is  the  duration  of  the  stance  phase.  An  additional  displacement 
is  needed  to  correct  the  errors  in  the  forward  speed: 

xfA  =  kfix-Xj)  (eq.  2.64) 


where  x^  is  the  displacement  of  the  foot  from  the  neutral  point,  x^  is  the  desired 

forward  speed  and  is  the  feedback  gain  constant. 

By  combining  the  equation  Eq.  2.64  and  Eq.  2.65,  the  total  foot  displacement  is 
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(eq.  2.65) 


*Ts 

xf  =  —  +  k.(x-xd) 


Then  the  required  hip  angle  for  the  desired  velocity  is 


.  (*Ts 

Y d  =  ®  -  arc  sm^—  + - - - 


(eq.  2.66) 


where  y  is  the  angle  between  the  leg  and  the  body  [RAIB86]. 

Another  research  focused  on  biped  systems  was  conducted  by  Troy  [TROY96].  He 
investigated  locomotion  of  dynamically  balanced  biped  mechanisms.  Different  multilink 
planar  and  spatial  biped  models  were  developed  by  using  feedback  control  for  balancing 
and  walking.  In  general,  his  work  extends  the  results  described  above  while  confining 
locomotion  to  a  specified  sagittal  plane. 


G.  SUMMARY 

This  chapter  provides  a  survey  of  previous  work  relating  to  modeling  and  control 
of  posture  and  gait  for  a  walking  human.  It  starts  with  kinematic  modeling  and  continues 
with  single  and  articulated  rigid  body  dynamic  models.  One  section  discusses  simulation 
of  postural  control  for  erect  posture  of  the  body.  The  last  part  of  the  chapter  illustrates  two 
different  stepping  control  approaches  for  a  given  desired  forward  speed.  The  next  chapter 
of  this  thesis  derives  the  differential  equations  of  a  human  model  which  has  two  degrees  of 
freedom  by  using  the  Newton-Euler  method.  It  also  discusses  postural  control  for  the  same 
model. 
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III.  DETAILED  PROBLEM  STATEMENT  AND  MATHEMATICAL 

FORMULATION 


A.  INTRODUCTION 

The  model  in  [GUBI74],  Figure  4,  is  designed  to  simulate  postural  and  gait  control 
with  three  degrees  of  freedom.  The  body  attitude  and  the  ankle  angle  are  used  for  postural 
control  and  the  variable  leg  length  and  foot  placement  as  well  as  periodic  stepping  are  used 
for  gait  control.  The  model  has  one  input  control  torque  and  one  input  control  force. 
Another  similar  model,  dealing  only  with  postural  control,  will  be  introduced  in  this 
chapter.  This  model  has  two  degrees  of  freedom  with  the  same  variable  angles  of  the 
previous  model.  Instead  of  the  input  control  force,  this  model,  shown  in  Figure  9,  has  a 
second  input  control  torque  at  the  ankle. 

B.  DESCRIPTION  OF  THE  MODEL 


Figure  9:  Single  Rigid  Body  Model  with  A  Constant  Length  Massless  Supporting  Leg 
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This  model,  Figure  9,  has  a  single  rigid  body  with  one  supporting  massless  leg.  The 
system  has  two  degrees  of  freedom.  The  state  vector  for  the  system  is 

X  =  (©j,  0j,  02,  02)r  (eq.  3.1) 

where  0  j  is  the  leg  angle  and  0^  is  the  body  attitude.  The  leg  length,  r ,  is  constant. 

The  input  vector  for  the  state  equations  is 
T 

u  =  («j,  Uy)  (eq.  3.2) 

where 


u  j  =  Mj 


and 


(eq.  3.3) 


u2 


(eq.  3.4) 


In  these  equations,  Mj  is  the  control  torque  applied  at  the  ankle  and  M2  is  the  control 


torque  applied  at  the  hip. 

The  input  vector,  u ,  is  used  to  control  leg  angle  and  body  attitude.  According  to 
general  linear  feedback  control 


u 


K(x-xq) 


(eq.  3.5) 


where  Xq  is  the  desired  state  vector.  A  special  form  of  this  equation,  called  local  feedback, 

has  been  proposed  for  postural  control  [KOOZ83][CAMA77].  In  local  feedback,  each  joint 
torque  is  determined  by  angle  and  rotation  rate  of  that  joint  only.  This  decouples  control  of 
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joints  from  each  other.  Specifically,  in  this  form  of  control,  the  leg  angle  is  controlled  by 
Wj  as 

u\  =  ^0,(01  _ocl) +^0^1  (eq.  3.6) 

where  Jcq^  and  are  gain  constants  and  OCj  is  the  desired  leg  angle.  The  body 
attitude  is  controlled  by  w2  as 

U2  =  Ve2~a2)  +  /ce,02  (eq.  3.7) 

where  and  are  gain  constants  and  CC2  is  the  bias  term  to  permit  the  desired 
body  attitude.  Local  control  will  be  examined  further  in  this  thesis. 

C.  THE  LAGRANGIAN  VERSION  OF  THE  PROBLEM 

The  components  of  the  linear  velocity  vector  of  the  center  of  mass  of  the  single  rigid 
body  can  be  derived  from  angular  variables  as  follows: 

y  =  r01cos01  + /02cos02  (eq.3.8) 

z  =  — r0j  sin0j— /02sin02  (eq.  3.9) 

The  kinetic  energy  of  the  system  is  thus 

1  .  2  1.  .  2 
K  =  -m(r0j  cos0j  +  /02cos02)  + -m(-r01sin01-/02sin02) 

1  ,  A2 

+  -/02  (eq.  3.10) 

The  potential  energy  of  the  system  can  be  expressed  as 

V  =  mg(rcos01  + /cos02)  (eq.3.11) 

Thus,  the  Lagrangian  function  becomes 
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i  #  2  1  .  2 

L  =  imCr©!  cos01  + /02cos02)  + -m(-r01sin01-/02sin02) 

z  z 


1  •  2 

+  -/02-mg(rcos01  +  /cos02) 

The  differential  equation  for  the  first  generalized  coordinate,  0  p  is 


(eq.  3.12) 


d  dL  dL 


^001  00] 


=  w,-m2 


(eq.  3.13) 


0L 

The  term,  — — ,  in  equation  Eq.  3.12  can  be  derived  as 
00] 


0L 

06^ 


2 

=  mr  0j(cos0|)2  +  /rar02COS0j cos02 


+  mr  0](sin01)2  +  Zmr02sin01  sin02 
Then,  the  first  term  in  the  equation  Eq.  3. 12  is 


d  dL  ,___A  ,2  -  2A2 


<if0Q] 


=  mr  0](cos0j)2 - 2 mr  ©jeosOjSin©^ 


+  /mr02COS01  cos02-/mr020]  sin0j  cos02 

•  2  2--  2 
-/mr02c os 0 j sin 02  +  mr  0](sin0) 

2-2 

+  2 mr  01sin0][cos02  +  m/r02Sin01sin02 

.2 

+  m/r020]  cos01  sin02  +  m/r02sin01  cos02 
And  the  second  term  in  the  equation  Eq.  3.12  is 


(eq.  3.14) 


(eq.  3.15) 


26 


dL 

80j 


2-2 

-mr  G^sinOjCOsOj  -mr/0i02sin01cos02 


2A2 


+  mr  0jcos01sin01  +  mr/0i02cos01sin02 


+  mgr sin0j  (eq.  3.16) 

By  substituting  Eq.  3.14  and  Eq.  3.15  into  Eq.  3.12,  the  following  equation  can  be  obtained: 
2- 

mr  0j  + /mr02(  sin  0j  sin  02  +  cos  0j  cos  02) 

.2 

+  /mr 02 ( sin  0  j  cos  02  -  cos  0j  sin  02) 

+  /mr020 1 ( cos 0j  sin 02-  sin01cos02) 


+  ImrQ  1 02(sin01cos02-  cosOjSM^) 

-mgrsinOj  =  -  M2  (eq.  3.17) 

After  applying  trigonometric  conversion  rules,  the  first  differential  equation  of  the  model 
can  be  derived  as  follows: 

2-  -  .2 
mr  0j  +  /mr02cos(02  -  0j)  +  /mr02sin(01  -  02) 

-mgr  sinOj  =  M^-M2  (eq.  3.18) 

The  differential  equation  for  the  second  generalized  coordinate,  02 ,  is 
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d dL  dL  , , 

— - —  =  Mn 

dt$Q  de2  2 


(eq. 3.19) 


dL 

The  term,  — — ,  equation  in  Eq.  3.19  can  be  derived  as 

302 


2  2 

— —  =  mlrd\  cosO^  cos02  +  ml  02(cos02) 

30o 


+  mlrd\ sdn6jsm62  +  m/20 2(sin02)2  +  70  2 

The  time  derivative  of  Eq.  3.20  is 

d  dL  ..  *2 

— — —  =  m/r01cos01cos02-m/r0i  8^0^0802 
dt  300 


(eq.  3.20) 


•  •  2-  2 
- m/r0102cos01  sin02  +  m/  02(cos02) 

2-2 

-  2m/  02cos02sin02  +  m/r0j  sin 0j  sin 02 


.2 


+  m/r0j  cosOjSii^  +  m/r0i02sin01  cos02 


+  m/202(sin02)z  +  2m/202sin02cos02  +  702 
Then  the  second  term  in  the  equation  Eq.  3.18  can  be  derived  as 
3  l 

-  =  -  m/r0]02cos01  sin02  +  m/r^j^sinOj  cos02 
302 


■  2 


(eq.  3.21) 


+  rag/sin02 


(eq.  3.22) 


By  substituting  Eq.  3.21  and  Eq.  3.22  into  Eq.  3.19,  the  following  equation  can  be  obtained: 
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•  •  •  z, 

mlrQ  i  cos  0 1  cos  02  -  mlrQ  \  sin0  ^  cos02 

•  •  2-  2 
-m/r0102COS0jSin02  +  ml  02(cos02) 

2-2 

-  2 ml  02cos02sin02  +  mlrQ j sin©,  sin02 

.2 

+  mZr©!  cos  0 !  sin  02  +  m/r0102sin01cos02 
+  mZ20 2(sin02)2  +  2raZ202sin02cos02  +  /02 
+  m/r0102cos01sin02-m/r0102sin01cos02 

+  mgZsin02  =  M2  (eq.  3.23) 

Then,  the  second  differential  equation  of  the  model  can  thus  be  expressed  as  follows: 

.2 

mlrQ\ cos(0j  -  02)  +  mlrQ\ sin(02 -  0^) 

2  •• 

+  (/  +  raZ  )02-mg/sin02  =  M2  (eq.  3.24) 

The  model  in  [GUBI74]  has  an  additional  degree  of  freedom  comparison  to  the 
model  presented  in  this  thesis  which  is  variable  leg  length  and  does  not  have  any  ankle 

control  torque.  If  the  time  derivative  values  of  the  leg  length  variable  r  are  taken  out,  and 

the  ankle  torque  Mj  is  added  to  the  dynamic  equations  in  [GUBI74],  it  is  seen  that  these 

equations  are  identical  with  the  equations  Eq.  3.17  and  Eq.  3.23. 
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D.  NEWTON-EULER  FORMULATION 


As  discussed  in  the  previous  chapter,  if  the  Lagrangian  method  is  used,  an  increase 
in  the  number  of  degrees  of  freedom  complicates  the  calculations  exponentially  for  an 
articulated  mechanism.  Even  though  the  number  of  variables  are  not  many  in  this  case,  by 
considering  that  future  research  is  likely  to  investigate  more  complex  models,  the  Newton- 
Euler  method  looks  as  if  a  better  approach  for  this  kind  of  problems. 

The  model,  presented  in  this  chapter,  can  be  divided  into  two  parts  to  simplify  the 
problem.  These  parts  are  the  massless  leg  and  the  body  itself. 

1.  The  Newton-Euler  Equations  of  the  Massless  Leg 


Figure  10:  Free  Body  Diagram  for  The  Massless  Leg 
Because  the  leg  has  no  mass,  it  is  only  possible  to  talk  about  the  static  equilibrium 
of  this  body  part.  The  equilibrium  equations  can  be  expressed  as 
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n 

Y  -  0  (eq.3.25) 

i  =  1 

and 

n 

^AfJ.-Frr  =  0  (eq.  3.26) 

i  =  1 

where  M-,  F ■  and  r  are  defined  in  Figure  10. 

Equation  Eq.  3.25  can  also  be  written  as 

Ml-M1-rFr  -  0  (eq.  3.27) 

so  the  reaction  force,  F  ,  is  given  by 

M\  ~M2 

Fr  =  - - -  (eq.  3.28) 

2.  The  Newton-Euler  Equations  of  the  Body 


y  i  i 


Figure  1 1 :  Free  Body  Diagram  of  The  Body 
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The  dynamic  equations  of  the  free  body  in  the  z  and  y  directions  can  be  derived  as 
m  'z  =  -  mg  +  FcosBj  -  F;.sin0  l  (eq-  3.29) 

and 

my  =  FsinGj -Frcos01  (eq.  3.30) 

For  the  angular  motion  of  the  free  body,  the  dynamic  equation  of  the  body  can  be  expressed 
as 

702  =  M2-/77rcos(02-01)  +  /77sin(02-01)  (eq.  3.31) 

3.  Combining  Body  and  Leg  Equations 

As  shown  in  Figure  9,  the  geometric  constraint  equations  for  the  model  can  be 
derived  as 

y  =  rsin0j  + /sin02  (eq.  3.32) 

and 

Z  =  TCOS0J  + /COS02  •  (eq.  3.33) 

The  second  time  derivatives  of  Eq.  3.30  and  Eq.  3.31  are  as  follows: 

.2  ..  -2 

y  =  r0j  cos0  j  —  r0]  sin0j  +  /02cos02  —  /02sin02  (eq.3.34) 

and 

.2  -2 

z  =  -rQjSinOj -T02COS0J -/02Sin02-/02COS02-  (eq.  3.35) 

The  constraint  force  F  in  equation  Eq.  3.29  can  be  eliminated  by  using  equation  Eq.  3.26 
resulting  in  the  expression 
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(eq.  3.36) 


w2  =  m2  - 


/(M1-M2)cos(02-61) 


+  ZFsin(02  -  0j) 


By  combining  Eq.  3.28  and  3.32,  the  following  equation  can  be  derived: 

.2 

m(rQ\ cos©-,  -rOjsinOj  +  /02cos02  -  /02sin02)  = 


=  FsinOj  +  F  r  COS0J 


On  the  other  hand,  after  substituting  z  with  Eq.  3.33,  Eq.  3.27  can  be  written  as 

.2  ..  .2 

m(-r01  sinOj  —  r0|  cos0  j  -  Z02sin02- Z02cos02)  = 


-  mg  +  F COS0J  -  Frsin01 


The  equation  Eq.  3.34  can  redefined  as 


MM  +  r)-lM , 

02/-FZsin(02-01)  =  — - - - -  cos(02  —  01 ) 

After  reorganizing,  Eq.  3.35  and  Eq.  3.36  become 

©imrcosOj  +  02m/cos02-Fsin01  = 


•  2  .2 
=  mrQi  sinOj  +  raZ02sin02  + 


mx-m2 


COS  01 


and 


0imrsin01  +  02m/sin02  +  Fcos©!  = 


.2  .2 

=  -rar0i  cos01-mZ02cos02  +  mg  + 


mx-m2 


sin  0i 


Equations  Eq.  3.37,  Eq.  3.38,  and  Eq.  3.39  can  be  expressed  in  matrix  form  as: 


(eq.  3.37) 


(eq.  3.38) 


(eq.  3.39) 


(eq.  3.40) 


(eq.  3.41) 
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0  1 
rarcos0j  m/cos02 
mrsin©!  m/sin02 


-sin©! 


COS©, 


0i 

e2 

F 

MM  +  r)-lM , 

- COS (©2  -  ©j) 

.2  .2 

mr©i  sinQj  +  m/©2sin©2  + - - - cos©j 

.2  .2  M\~M2  . 

- mr©j cos©!  -  ml$2C0S®2  +  - sin©! 


(eq.  3.42) 


An  alternative  to  this  analytic  formulation  of  the  Newton-Euler  formulation  is  to 
use  the  recursive  approach  explained  in  Chapter  II  of  this  thesis.  Thus  results  in  more 
complex  logic,  but  in  less  computation,  especially  as  the  number  of  links  in  the  dynamic 
model  increases. 


E.  LINEARIZED  ANALYSIS  FOR  CHOOSING  GAINS 

As  mentioned  in  the  previous  sections,  this  system  has  two  inputs,  hip  and  ankle 
torques.  The  main  purpose  of  postural  control  is  to  determine  these  input  torques  in  order 
to  maintain  the  upright  orientation  of  the  body.  The  approach  taken  in  this  thesis  is  linear 
state  feedback  control.  The  control  equations,  Eq.  3.5  and  Eq.  3.6,  are  presented  in  the 
second  section  of  this  chapter.  For  the  local  control  model,  the  problem  is  to  compute  the 

required  gain  values:  Jcq^,  ,  k k^  . 

It  is  reasonable  to  drop  quadratic  velocity  components  for  small  motion  linearized 

•  2  -2 

analysis  [MCGH86].  Under  this  assumption,  ©j  and  ©2 ,  components  are  removed  from 
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all  equations.  The  same  assumption  means  that  0j  and  02  can  have  only  small  values 

which  allows  the  substitutions 

sin01  =  0p 

(eq.  3.43) 

sin02  =  02, 

(eq.  3.44) 

COS0J  =  1 , 

and 

(eq.  3.45) 

cos02  =  1 . 

With  these  considerations,  equation  Eq.  3.18  can  be  rewritten  as 

(eq.  3.46) 

2  •• 

mr  Q\+lmr<d 2  =  mgrQ^  +  M2 . 

The  linearized  version  of  equation  Eq.  3.24  is 

(eq.  3.47) 

mlrQ\  +  (/+  w/2)©2  =  mg/02  +  M2 

02  can  be  evaluated  from  equation  Eq.  3.47  as 

mgrQ j  +  My  -  M2  -  mr^Q j 

02  ”  Imr 

(eq.  3.48) 

(eq.  3.49) 

If  equation  Eq.  3.49  is  substituted  in  equation  Eq.  3.48,  the  following  equation  is  obtained 

(mlr)  0j+(/+m/  )(mgrQ^  +  .M|  —  M2~ 

2  2 

=  rm  gl  02  +  mlr  M2 

After  reorganizing,  equation  Eq.  3.50,  0 1  can  be  defined  as 

01  = 

35 

(eq.  3.50) 

=  /2^2^ge2-mgr(/+m/2)ei  -{I+ml2)M]  +  (Imr  +  /  +  W/2)M9 

— . Imr 2 

(eq. 3.51) 

The  angular  acceleration,  9  j ,  can  be  defined  by  using  equation  Eq.  3.47  as 


..  mgr®l  +  -M2-lmrQ2 

9i  - - 

4 


(eq.  3.52) 


mr 


After  substitution  of  9  j ,  equation  Eq.  3 .48  can  be  rewritten  as 


/ 


^(mgr9j  +  Mj  -  M2  - /mr§2)  +  (/+ w/  )92  =mglQ2  +  M?  (eq.  3.53) 


After  reorganizing  equation  Eq.  3.53,  Q2  can  be  define  as 


9- 


mglrQ2  -  lmgrQl  -  +  (r  +  l)M. 


2 


rl 


(eq.  3.54) 


The  general  linear  state  feedback  equation  is 

x  =  Ax  +  Bu  (eq.  3.55) 

The  state  vector,  x ,  is  defined  by  equation  Eq.  3.1.  If  the  desired  values  for  the  angles  are 
chosen  as  zero,  the  input  vector,  in  equation  Eq.  3.5,  can  be  rewritten  as 
u  =  Kx 


(eq.  3.56) 


After  substituting  u  according  to  the  equation  Eq.  3.56,  Eq  3.53  can  be  written 


as 
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x  =  (A  +  BK)x 


(eq.  3.57) 


where 


A  = 


0  10  0 

2  2  2 

mgr(I  +  ml  )  l  m  rg 

2  U  2  U 

Imr  Imr 

0  0  0  1 

-Imgr  0  mglr 

Ir  Ir 


and 


(eq.  3.58) 


B  = 


0 


0 


I  +  ml  -(Imr  +  I  +  ml  ) 


Imr 

0 

_J_ 

Ir 


Imr 

0 

r  +  l 
Ir 


(eq.  3.59) 


For  local  feedback,  K  can  be  defined  as  follows,  according  to  the  equations  Eq.  3.2,  Eq. 
3.6  and  Eq  3.7 


-. K a  -K.  0  0 

yi  0i 

0  0  -K*  -K. 

y2  02 


(eq.  3.60) 


where  desired  angle  values  Ot^  and  are  taken  as  zero.  Then,  by  using  equation  Eq. 
3.58,  Eq.  3.59  and  Eq.  3.60,  A  +  BK  can  be  defined  as 
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where 


A  +  BK  = 


0  10  0 
M  N  O  P 
0  0  0  1 
Q  V  Y  Z 


(eq.  3.61) 


2  2 

mgr{l  +  ml  )-Kq  ( I  +  ml  ) 

M  =  - ^ -  (eq.  3.62) 

Imr 


N  = 

(eq. 3.63) 

/mr2 

0  = 

2  2  2 

-l  m  rg  +  K q  {Imr  +  I  +  ml  ) 

Imr 2 

(eq.  3.64) 

p  — 

2 

iT .  (//nr  +  7  +  m/  ) 

02 

(eq.  3.65) 

/mr2 

Q  = 

-  /mgr  + 

rl 

(eq.  3.66) 

v  = 

V 

rl 

(eq.  3.67) 

Y  = 

mglr-K§{r  +  1 ) 

rl 

(eq. 3.68) 

7  — 

-Ki>lr  +  0 

(eq.  3.69) 

Zj  — 

rl 
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For  a  stable  upright  posture,  all  roots  of  the  characteristic  equation  (eigenvalues) 
should  have  negative  real  parts  [KU095].  The  characteristic  equation  of  the  closed  loop 
system  is  [KU095] 

|  J/-(A  +  BK)  |  =  0  (eq.  3.70) 

After  substituting  the  equation  Eq.  3.60  the  equation,  Eq.  3.69  can  be  rewritten  as 


5 

-1 

0 

0 

c 

D 

E 

F 

0 

0 

s 

-1 

G 

H 

W 

Y 

where 


C  = 


2  2 
-  mgr(I  +  ml  )  +  Kq  (/  +  ml  ) 


Imr 


(eq.  3.71) 


(eq.  3.72) 


D 


s  + 


K^(I  +  mf) 

2  ’ 

Imr 


(eq.  3.73) 


2  2  2 
l  m  rg-K q  ( lmr  +  I  +  ml  ) 

E  =  - - - 2 - ’  (eq.  3.74) 

Imr 

2 

-K.  ( Imr  + 1  +  ml  ) 

F  =  - 1 - 2 - ’  (eq.  3.75) 

Imr 


G  = 


Imgr-K Q  l 

7i 


(eq.  3.76) 
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H  = 


-w 

rl  ’ 


(eq.  3.77) 


W  = 


-Imgr  +  Kq  ( r  +  l ) 

7i  ’ 


(eq. 3.78) 


Y  =  S  + 


Vr+/) 

rl 


(eq.  3.79) 


If  the  determinant  of  in  the  equation  Eq.  3.69  is  evaluated,  the  following  polynomial  is 
obtained: 

5  (r  Im) 

+  s3(lrmK.  +  IK.  +  K.  ml 2  +  /mK.  1 

V  e2  e,  9i  e2J 

+  5,2(/i^0i  -  l^rn^rg  +  -  P'm'lg 


+  r  mK*  +  rmlKa  +K.  K.  -Imgr) 

02  02  01  02 

+^e,V^e,  +  i:e1V”«rV 

+  (m2g2rl-mgrK9i-K()ilmg  +  KeK eJ  =  0 


(eq.  3.80) 


It  is  clearly  a  very  hard  problem  to  determine  the  required  gain  values  in  equation  Eq.  3.80, 
which  result  in  all  roots  having  negative  real  parts.  Instead,  a  experimental  solution  was 
found  by  trying  some  gain  values  on  the  computer  model.  It  was  found  that  the  following 
values  produce  an  upright  stable  body  posture: 
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=  25000, 

(eq.  3.81) 

K.  =  2500, 

6i 

(eq. 3.82) 

Kq  =  4000, 

(eq.  3.83) 

K.  =  400 

02 

(eq.  3.84) 

F.  SUMMARY 

In  this  chapter,  a  new  human  dynamic  model,  which  is  made  of  a  body  and  a 
massless  leg,  is  introduced.  The  dynamic  differential  equations  of  the  model  are  derived  by 
using  two  different  methods:  the  Lagrangian  and  the  Newton-Euler  methods.  The  last 
section  of  the  chapter  discusses  the  small  motion  linearized  analysis  of  the  system.  It  is 
explained  how  the  gain  constants  for  the  input  torques  should  be  chosen. 

The  next  chapter  will  explain  the  implementation  of  computer  models  based  on  the 
knowledge  presented  in  the  second  and  third  chapters  of  this  thesis. 


41 


42 


IV.  COMPUTER  MODELS 


A.  INTRODUCTION 

In  the  second  chapter  of  this  thesis,  it  is  pointed  out  that  there  exist  two  major 
methods  to  simulate  human  motion  simulation:  kinematic  and  dynamic  models.  The 
present  chapter  first  presents  stepping  algorithms  for  a  human  figure  implementation  which 
is  developed  by  using  a  kinematic  model.  The  second  chapter  also  discusses  the  general 
approaches  for  dynamic  simulation  of  human  motion.  Based  on  this  knowledge,  the  third 
chapter  investigates  the  mathematical  representation  of  a  dynamic  human  model.  The 
present  chapter  also  introduces  the  computer  implementations  of  these  models. 

B.  KINEMATIC  COMPUTER  MODEL  (DYNAMAN) 


Figure  12:  Kinematic  Computer  Model:  Dynaman 
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The  kinematic  model  presented  in  this  section  is  made  of  fifteen  separate  body  parts 


as  shown  in  Figure  13. 


o 


DCS  7 


15 

16 


Figure  13:  The  Body  Parts  and  Corresponding  Dynamic  Coordinate  Systems  (DCS)  of 

Dynaman 

The  purpose  of  the  simulation  is  to  create  a  computer  representation  of  a  stepping 
human.  Under  this  requirement,  it  is  clear  that  the  main  concern  is  leg  and  foot  motion.  The 
other  body  parts  are  synchronized  according  to  the  legs. 

Inverse  kinematics  is  chosen  for  computing  leg  motion.  Inverse  kinematic 
equations  take  the  position  of  the  end  effector  (foot)  as  the  input  and  computes  each  joint 
angle  of  the  leg.  The  points  which  describe  the  position  of  the  foot  in  space  for  a  full  gait 
period  produce  a  path.  The  question  is  to  define  this  path  as  a  mathematical  function  of 
time.  This  function,  of  course,  should  be  developed  with  the  knowledge  of  the  geometry  of 
the  environment,  such  as  height  of  each  stair. 

Forward  kinematics  could  have  been  chosen  for  the  same  problem.  In  that  case, 
joint  angles  of  the  leg  would  be  the  input  to  the  forward  kinematic  equations,  and  the 
position  of  the  end  effector  would  be  calculated.  For  this  second  case,  there  should  exist  a 
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feedback  system  in  order  to  input  the  constraints  of  the  environment  to  the  model.  A 
collision  detection  mechanism  between  the  foot  and  the  floor  seems  to  be  appropriate  to  this 
approach  [GOET94], 

1.  Inverse  Kinematic  Equations  for  Three  Link  Planar  Manipulator 


Figure  14:  Three  Link  Planar  Manipulator 

The  leg  of  Dynaman  can  be  thought  as  a  three  link  planar  manipulator  which  is 
made  of  the  upper  leg,  the  lower  leg,  and  the  foot.  The  frames  which  are  attached  to  the 
links  are  shown  in  Figure  14.  The  general  form  of  the  transformation  matrix  to  represent  a 

point  in  the  frame  i —  1  which  is  defined  in  frame  i  is  [CRAI89] 
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(eq.  4.1) 


where  a,  (X ,  0 ,  and  d  are  the  link  parameters  as  defined  in  the  second  chapter  of  this 
thesis.  The  link  parameters  for  the  presented  model  are  defined  in  Table  1. 
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Table  1.  Link  Parameters  of  the  Leg  [CRAI89] 


In  this  model,  all  axes  are  parallel,  and  all  x-  axes  are  in  the  same  plane.  That  is 
why  all  the  Ct-  _  j  and  fiT  values  are  zero.  The  parameters  /  j  and  /y  are  the  link  lengths 


of  the  first  and  second  links.  The  parameter  defines  the  position  of  the  toe  point.  Since 

it  is  in  the  end  effector  frame,  it  is  not  included  in  the  link  parameters. 

According  to  equation  Eq.  4.1  and  Table  1,  the  transformation  matrices  for  the 
neighbor  links  are 


cos01  -sinGj  0  0 

Oy,  _  sinGj  cosGj  0  0 

0  0  10 
0  0  0  1 


(eq.  4.2) 
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(eq.  4.3) 


(eq.  4.4) 


The  transformation  matrix  between  the  first  and  the  last  links  can  be  derived  by  using  the 
product 

1_  2 

3^  —  2^  3^  (eq.  4.5) 


If  the  matrix  multiplications  are  executed  after  substituting  equations  Eq.  4.2,  Eq.  4.3,  and 
Eq.  4.4  into  the  equation  Eq.  4.5,  the  following  transformation  matrix  will  be  obtained: 


cos(0j  +  ©2  +  03)  — sinCOj  +  02  +  O3)  0 

sin(0j  +  @2  +  0^)  cos(0j  +  ©2  +  ©3)  0 

0  0  1 

0  0  0 


lfosQl+l2cos(Ql  +  ©2) 

/1sin©1  +  /2sin(01  +  ©2) 

0 

1 


(eq.  4.6) 

If  the  method  in  [CRAI89]  is  used,  the  transformation  matrix  of  the  end  effector 
according  to  the  base  link  can  be  defined  as 
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(eq.  4.7) 


0 

3 


T  = 


cosG  -sinG  0  x 
sin0  cos0  0  y 
0  0  10 
0  0  0  1 


where  the  position  components,  x  and  y ,  and  the  orientation,  0 ,  of  the  end  effector 
according  to  the  base  link  are  known.  The  following  equations  can  be  derived  from 
equations  Eq.  4.6  and  Eq.  4.7: 


COS0  =  cos(0j  +  ©2  +  0^) 

(eq.  4.8) 

sin©  =  sin(0  j  +  02  +  03) 

(eq.  4.9) 

x  =  Z1cos01  +  Z2cos(01  +  02) 

(eq.  4.10) 

y  =  ZjSinOj  +  Z2sin(01  +  02) 

(eq.  4.11) 

If  the  squares  of  equations  Eq.  4.10  and  Eq  4.11  are  added,  the  following  equation  is 
obtained: 

x2+y2  =  l\  +  l22  +  2lxl2COS$2  (eq.  4.12) 

In  equation  Eq.  4. 12,  the  only  unknown  is  ©2  and  it  can  be  defined  as 


02  -  acos 


,2  2  .  2  .  2x 

fx  +y  -Zj  -Z2  ) 
2/^2 


(eq.  4.13) 


V  1  *  J 

After  having  found  02 ,  equations  Eq.  4.10  and  4. 1 1  can  be  can  be  written  as 

x  =  /cj  cosOj  -  Z^sinOj 


(eq.  4.14) 


and 
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y  =  ^sinGj  +^2cos0j 


(eq.  4.15) 


where 


&1  -  1 /2cos^2 


(eq.  4.16) 


and 


&2  =  1 2  s*n®2 


(eq.  4.17) 


After  following  the  steps  in  [CRAI89],  0  j  is  defined  by  the  equation 


0j  =  atan  -  J-atan 


fk2^ 

vkij 


(eq.  4.18) 


Then  foot  angle  can  then  be  calculated  as 


@2  =  0  —  0  j  —  @2 


(eq.4.19) 


2.  Link  Descriptions  in  the  Computer  Model 

The  computer  model  has  been  developed  in  the  IRIS  Performer™  application 
environment.  This  environment  allows  the  programer  to  create  a  Dynamic  Coordinate 
Systems  (DCS).  DCS  lets  the  programer  to  change  the  transformation  of  the  objects  which 
are  attached  to  that  DCS  node.  The  method  followed  in  this  application  is  to  attach  each 
body  part  to  a  DCS  node  and  connect  all  the  DCS  nodes  in  a  tree  structure  which  creates  a 
hierarchical  structure.  For  example,  if  the  upper  leg  is  rotated  for  some  degrees,  all  the  body 
parts  under  that  DCS  node,  lower  leg  and  foot,  follow  this  rotation.  DCS  tree  structure  of 
the  whole  body  is  shown  in  Figure  13  and  Figure  15. 
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Figure  15:  Dynamic  Coordinate  System  (DCS)  Hierarchy  Tree  of  Dynaman 

The  body  parts  of  the  Dynaman  are  taken  from  an  other  model  developed  in 

OpenGLR  by  Will  Frey  at  the  Naval  Postgraduate  School  [FREY96].  However,  while  Frey 
used  body  segment  Euler  angles  relative  to  an  Earth  fixed  reference  system,  as  described 
above,  the  work  of  this  thesis  is  based  on  joint  angles.  The  coordinates  of  the  polygons, 
taken  from  the  other  model,  which  produces  the  body  parts,  are  loaded  to  the  application 
by  using  poly  format  files. 

3.  Stepping  Algorithms 

The  inverse  kinematics  model  takes  position  and  orientation  of  the  end  effector  and 
computes  each  joint  angle.  Then,  the  question  is  to  define  the  path  of  the  end  effector  (foot) 
which  is  the  input  to  the  inverse  kinematics  model.  Two  separate  algorithms  are  developed 
to  define  the  path:  stepping  forward  and  stepping  upward  algorithms. 

a.  Stepping  Forward  Algorithm 

The  leg,  without  bending  at  the  knee,  can  be  assumed  as  a  simple  swinging 
bar  on  a  circular  path  which  is  shown  as  P  j  in  Figure  16.  The  angle  between  the  vertical 
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and  the  swinging  bar  is  defined  as  (X ,  which  takes  values  between  -20  and  +20  degrees 
during  the  gait  cycle.  In  reality,  the  knees  are  bent  during  stepping.  Larger  values  of  a 

introduce  larger  bending  angles  at  the  knee.  When  a  is  equal  to  zero,  knee  bending  angle 
should  also  be  zero.  This  requirement  is  introduced  by 

P2  =  P^c  OS  a  (eq.  4.20) 

There  is  a  difference  between  the  supporting  and  the  recovery  leg  motions.  While 
the  supporting  leg  always  touches  the  ground,  the  recovery  leg  should  be  moved  without 
touching  the  ground.  This  requirement  is  added  to  the  model  by  using  the  following 
equation  for  the  recovery  leg 

P3  =  0.95  P2  (eq.  4.21) 


Figure  16:  Forward  Stepping  Algorithm 
No  kinematic  computation  is  needed  for  the  arms.  They  are  synchronized 
with  the  legs.  The  arm  rotation  is  not  exactly  the  same  as  leg  rotation;  a  scale  factor  is 
applied  for  a  more  realistic  looking  arm  motion.  Another  property  of  the  model  is  the 
rotation  of  the  upper  body  around  the  vector  described  by  the  general  direction  of  motion. 
This  motion  is  caused  by  the  roll  moments  which  are  generated  by  the  nature  of  biped 
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locomotion  since  the  supporting  legs  have  offset  from  the  center  of  mass  on  opposite  sides 
and  are  alternating  during  each  step  cycle. 


Figure  17:  Upper  Body  Rotation 

The  distance  from  the  upper  body  to  the  ground  is  not  constant.  When  the 
a  parameter  has  larger  values,  the  whole  body  gets  closer  to  the  ground.  This  property  is 
implemented  by  moving  the  whole  body  vertically  with  a  cosine  function  of  time. 


Figure  18:  Change  in  the  Height  of  the  Body  During  One  Gait  Cycle 


The  height  of  the  upper  body  is  calculated  by 

Height  =  0.08  cos  (27i/f) 


(eq.  4.22) 


b.  Stepping  Upward  Algorithm 

The  stepping  up  algorithm  has  a  major  difference  from  the  forward  stepping 
which  is  caused  by  the  need  for  increase  in  body  elevation 


Figure  19:  The  Amount  of  Elevation  For  Stepping  Up 
The  needed  elevation  for  stepping  up  is 

h  =  (/j  + /2)  ~  (^i  + /2) COS  (3.  (eq.  4.23) 

This  elevation  can  only  be  handled  by  raising  the  foot  to  a  higher  position  in  the  front.  This 
causes  the  change  which  is  shown  in  Figure  20.  The  values  of  OC  change  from  -10  to  +30 
degrees  for  the  gait  cycle. 
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P  3 


Figure  20:  Stepping  Up  Algorithm 

C.  DYNAMIC  COMPUTER  MODELS 

1.  Newton-Euler  Rigid  Body  Class 

The  Dynamic  models  in  this  thesis  are  developed  in  ANSI  Common  Lisp.  The  main 
class  for  the  dynamic  simulation  is  rigid-body  class  which  was  written  by  Professor 
McGhee.  This  Lisp  code  for  this  class  is  included  as  Appendix  A  of  this  thesis.  This  class 
defines  a  free  rigid  body  in  space.  The  major  method  of  the  rigid-body  class  is  update-rigid- 
body  which  updates  the  posture  vector  which  includes  the  position  and  the  orientation  of 
the  rigid  body  in  an  earth  coordinate  system.  This  method  converts  body  velocity  rates  to 
earth  velocity  rates  to  update  the  six  element  posture  vector.  Euler  integration  is  used  to 
update  body  velocities  by  using  body  velocity  growth  rates.  The  body  velocity  growth  rates 
result  from  applied  forces  and  torques  on  the  body.  The  linear  velocity  growth  rate  is 
computed  by  [FRAN69] 
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(eq.  4.24) 


where  /  ,  f  ,  /  is  the  force  vector  applied  to  the  body,  (u,  v,  w)  is  the  linear  velocity 
■X  ~y  z 

vector,  (p,  q ,  r)  is  the  rotational  velocity  vector,  ((f),  0,  \| /)  is  the  body  orientation 

defined  in  Euler  angles,  m  is  the  mass,  and  g  is  the  gravitational  acceleration.  All  these 
vectors  are  defined  in  a  body  principal  axis  coordinate  system  [FRAN69].  The  angular 
velocity  growth  rate  is  computed  by 
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4 
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^ XX 

7yy 


(eq.  4.25) 


where  (L,  M,  N)  is  the  torque  vector  applied  to  the  body,  I xx,  I  ,  and  I _  are  mass 

moments  of  inertia,  ( p ,  q,  r)  is  the  rotational  velocity  vector  [FRAN69].  In  summary,  the 
update-rigid-body  method  calculates  the  new  position  and  orientation  of  the  free  rigid  body 
in  an  earth  coordinate  system  according  to  the  applied  forces  and  torques  to  the  body.  An 
other  important  method  is  move  which  takes  the  displacement  components  and  orientation 
Euler  angles  as  input  and  moves  the  body  according  to  the  inputs. 
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2.  Numerical  Integration  Methods 

Two  different  integration  methods  are  used  in  the  dynamic  modeling 
implementations:  Euler  and  Huen  integration.  The  Euler  integration  approach  can  be 
formalized  as  [KREY88] 

+  1  =  xn  +  /<*„ '  'n>A'  <«!•  4'26> 

where 

y  =  f(xn,  t)  (eq.  4.27) 

and  At  is  a  constant  increment  in  the  independent  variable,  t. 

The  Heun  integration  formula  can  be  defined  with  the  equation  [KREY88] 

"n  +  1  =  '„)  +  /(*„+  l*’fn+l)]A  <eq.  4.28) 

where 

S.+  1*  =  xn+f<-xn’tn^AL  (eq,4.29) 

The  symbols  At  and  f(x  ,  t  )  have  the  same  definitions  for  Heun  integration  as  in  Euler 
integration. 

3.  Dynamic  Inverted  Pendulum  Simulations 

Three  different  dynamic  inverted  pendulum  models  are  implemented  in  this  thesis: 

a.  A  Single  Link  Single  Rigid  Body  with  Newton-Euler 
The  second  implementation  simulates  an  inverted  pendulum  by  using  Newton- 
Euler  formulation  of  the  dynamic  equations  of  the  system.  The  constraint  forces  from  the 
dynamic  equations  and  the  control  torque  are  the  inputs  of  the  rigid-body  class.  The  3x3 
matrix  multiplication  form  of  the  system  equations  is  as  follows: 


56 


0 

M 

/ 

-l cos©  Zsin0 

ra/cos0 

1  0 

ef 

X 

= 

.2 

0  m/sin0 

m/sin0 

0  -1  _ 

£_ 

A2  r  „ 

LCj 

mg  -  0  ml cos0 

where  all  the  variables  are  defined  as  in  Figure  21. 


(eq.  4.30) 


Figure  2 1 :  Inverted  Pendulum  with  Constraint  Forces 
The  system  has  only  one  degree  of  freedom.  A  suitable  state  vector  is 

x  =  [0,  0]  (eq.  4.31) 

To  assume  M  =  0 ,  makes  the  system  behave  like  a  natural  inverted  pendulum 
without  control.  By  using  linear  state  feedback,  the  control  moment  can  be  determined  as 


M=-KJ-Kj 


(eq.  4.32) 


■<r  “f 

where  K,  and  K  ■  are  control  gain  variables.  Suitable  values  for  K>  and  K  ■  can  be 
9  (j)  9  <j) 

found  analytically  for  small  motion  linearization  of  this  system  because  its  characteristic 

equation  is  quadratic.  The  Lagrangian  formulation,  Eq  2. 27,is  most  useful  for  this  purpose. 
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b.  Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian 

The  second  implementation  simulates  a  two  link  structure  which  is 
presented  in  the  third  chapter  of  this  thesis.  The  dynamic  equations  of  the  system  are 
defined  with  Eq.  3.18  and  Eq.  3.24  which  are  derived  by  using  Lagrangian  method.  The 
linear  state  feedback  control  equations  are  given  by  the  equations  Eq.  3.6  and  Eq.  3.7.  The 
state  vector  is  defined  as  Eq.  3.1. 

c.  Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler 

The  third  implementation  simulates  the  same  two  link  structure  which  is 
simulated  in  the  second  simulation.  However  the  Newton-Euler  method  is  used  to  derive 
the  dynamic  equation  instead  of  the  Lagrangian.  The  3x3  matrix  form  of  the  dynamic 
equations  are  given  by  Eq.  3.42.  The  linear  state  feedback  control  equations  and  the  state 
vector  are  defined  as  the  same  as  in  the  Lagrangian  version  of  the  simulation. 

D.  SUMMARY 

This  chapter  presents  forward  and  upward  kinematic  stepping  algorithms,  of  an 

human  model  which  is  developed  at  the  IRIS  Performer™  application  environment  by 
using  C++.  The  second  part  of  the  chapter  explains  three  different  implementations  to 
simulate  various  inverted  pendulum  models  developed  in  Lisp.  The  next  chapter  discuses 
the  results  of  these  simulations. 
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V.  RESULTS  OF  COMPUTER  SIMULATIONS 


A.  INTRODUCTION 

In  this  chapter,  the  results  of  the  dynamic  simulations  are  presented.  The  second 
part  of  the  chapter  contains  frames  from  the  kinematic  simulation  of  Dynaman  which  show 
the  model  stepping  forward  and  upward. 

B.  DYNAMIC  SIMULATIONS 

1.  A  Single  Link  Single  Rigid  Body  with  Newton-Euler  Method 

Figure  22,  Figure  23,  and  Figure  24  show  the  behavior  of  the  inverted  pendulum 
with  a  control  torque  at  the  pivot  point.  The  dynamic  equations  of  the  system  are  derived 

by  using  the  Newton-Euler  Method.  The  mass  of  the  rigid  body,m ,  is  100  lb.  The  body 

rotary  inertia,  /,  is  900  lb.  ft.2.  The  length  of  the  inverted  pendulum,  Z,  is  3  ft.  The 

gravitational  acceleration,  g ,  is  32.2185  ft.  /  sec.  .  The  gain  values  for  the  control  torque 
are 

Kq  =  10000  (eq.  5.1) 

K.  =  2000  (eq.  5.2) 

0 

The  initial  state  vector  is 

*=(1,0)  (eq.  5.3) 

and  the  state  vector  in  steady  state  is 

*  =  (0,0)  (eq.  5.4) 

Figure  25  shows  inverted  pendulum  orientation  change  in  time.  Euler  integration 
with  a  time  step  of  0.02  seconds  was  used  for  the  results  shown  in  Figure  22-25. 
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Figure  22:  Initial  Position  and  Orientation  of  the  Inverted  Pendulum 


Mil 


Figure  23:  Inverted  Pendulum  Moving  to  the  Upright  Orientation 
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Figure  24:  Inverted  Pendulum  After  Steady  State 


Figure  25:  Body  Attitude  Response  of  the  Inverted  Pendulum 
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2.  Massless  Leg  and  a  Single  Rigid  Body  with  Lagrangian  Method 

Figure  26,  Figure  27,  Figure  28,  and  Figure  29  show  the  behavior  of  the  two  link 
inverted  pendulum  with  control  torques  at  the  hip  and  at  the  ankle.  The  z  axis  of  the  body 
is  drawn  to  be  able  to  observe  the  body  attitude  and  the  leg  angle  separately.  The  dynamic 
equations  of  the  system  is  derived  by  using  the  Lagrangian  Method.  The  mass  of  the  rigid 

body,m ,  is  100  lb.  The  body  rotary  inertia,  I ,  is  100  lb.  ft.2.  The  length  of  the  leg,  r ,  is  3 

ft.  The  length  of  the  rigid  body,  l,  is  0.5  ft.  The  gravitational  acceleration,  g ,  is  32.2  ft.  / 

sec.2.  The  gain  values  for  the  control  torques  are 


Kq ]  =  25000  (eq.  5.5) 

K.  =  2500  (eq.  5.6) 

0i 

KQ  =  40000  (eq.  5.7) 

K.  =  400  (eq.  5.8) 

02 

The  initial  state  vector  is 

x  =  (0.5,0,  1,0)  (eq.  5.9) 

and  the  state  vector  in  steady  state  is 

X  =  (0,0,  0,0)  (eq.  5.10) 


Figure  30  and  Figure  3 1  show  body  attitude  and  leg  angle  changes  in  time.  Heun 
integration  with  a  time  step  of  0.01  seconds  was  used  for  the  results  shown  in  Figure  26-3 1 . 


62 


Figure  26:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 


Figure  27:  Two  Link  Inverted  Pendulum  Moving  to  the  Upright  Orientation  by  Control 

Torques(Lagrangian) 
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Figure  28:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Lagrangian) 


Figure  29:  Two  Link  Inverted  Pendulum  After  Steady  State  (Lagrangian) 
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Figure  30:  Body  Attitude  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 


Figure  3 1 :  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Lagrangian) 
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3.  Massless  Leg  and  a  Single  Rigid  Body  with  Newton-Euler  Method 

Figure  32,  Figure  33,  Figure  34,  and  Figure  35  show  the  behavior  of  the  two  link 
inverted  pendulum  with  control  torques  at  the  hip  and  at  the  ankle.  Again,  the  z  axis  of  the 
body  is  drawn  to  be  able  to  observe  the  body  attitude  and  the  leg  angle  separately.  The 
dynamic  equations  of  the  system  are  derived  using  the  Newton-Euler  Method.  The  mass  of 

the  rigid  body,m ,  is  100  lb.  The  body  rotary  inertia,  / ,  is  100  lb.  ft.2.  The  length  of  the  leg, 

r,  is  3  ft.  The  length  of  rigid  body,  /,  is  0.5  ft.  The  gravitational  acceleration,  g ,  is  32.2  ft. 

/  sec.2.  The  gain  values  for  the  control  torques  are 


£0  =  25000 

(eq.  5.1 1) 

K  ■  =  2500 

e, 

(eq.  5.12) 

Kr,  =  40000 
u2 

(eq. 5.13) 

K  .  =  400 

(eq.  5.14) 

e2 

The  initial  state  vector  is 

x  =  (0.5,0,  1,0) 

(eq. 5.15) 

and  the  state  vector  in  steady  state  is 

x  =  (0,  0,  0,  0) 

(eq.  5.16) 

Figures  32  through  37  show  body  attitude  and  leg  angle  changes  in  time.  Heun 
integration  with  a  time  step  of  0.01  seconds  was  used  for  these  results. 
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Figure  32:  Initial  Orientation  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler) 


Two  Link  Inverted  Pendulum  (Constant  Length  Massless  Leg)  NEWTON-EULE 


Figure  33:  Two  Link  Pendulum  Moving  to  the  Upright  Orientation  (Newton-Euler) 
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Figure  34:  Two  Link  Pendulum  Recovering  the  Negative  Orientations  (Newton-Euler) 


Figure  35:  Two  Link  Inverted  Pendulum  In  Steady  State  (Newton-Euler) 
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Figure  36:  Body  Attitude  Time  Response  of  the  Two  Link  Inverted  Pendulum  (Newton- 

Euler) 


!  Leg  Angle  (theta-1)  NEWTON-EULER 

Figure  37:  Leg  Angle  Response  of  the  Two  Link  Inverted  Pendulum  (Newton-Euler) 
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C.  RESULTS  OF  THE  DYNAMIC  SIMULATION 


Euler  integration  was  used  for  the  single  link  inverted  pendulum  simulation.  For  the 
two  link  inverted  pendulum  simulations,  Heun  integration  is  chosen.  Heun  is  a  more 
accurate  method  than  Euler,  because  it  converges  quadratically  as  the  step  size  is  decreased, 
while  Euler  integration  converges  only  linearly. 

As  seen  in  the  previous  section,  the  Newton-Euler  and  Lagrangian  solutions  of  the 
two  link  inverted  pendulum  problem  give  identical  results.  However,  the  running  times  of 
these  simulations  are  not  the  same.  The  time  needed  to  complete  the  Newton-Euler  version 
of  the  simulation  is  20  percent  longer  than  the  Lagrangian  version.  This  is  an  expected 
consequence  of  the  matrix  inversion  which  takes  place  in  the  Newton-Euler  version.  The 
Lagrangian  version  of  the  problem  computes  the  accelerations  without  matrix  inversion. 
However,  it  is  hard  to  derive  dynamic  equations  using  Lagrangian  methods  for  more 
complex  models  with  higher  degrees  of  freedom.  Moreover,  due  to  the  complexity  of  such 
equations,  it  is  suspected  that  Newton-Euler  models  for  postural  control  may  run  faster  for 
more  complex  systems.  This  will  certainly  be  true  if  O(n)  methods  are  used  [MCMI95]. 

D.  KINEMATIC  SIMULATION  OF  STEPPING  DYNAMAN 

1.  Stepping  Forward  Algorithm 

Figures  38  through  43  are  six  frames  from  the  kinematic  model  simulation.  These 
six  frames  show  one  step  cycle  of  Dynaman  during  forward  stepping. 


70 


Figure  38:  Forward  Stepping  (1) 


Figure  39:  Forward  Stepping  (2) 


Figure  40:  Forward  Stepping  (3) 


Figure  41 :  Forward  Stepping  (4) 


Figure  42:  Forward  Stepping  (5) 


Figure  43:  Forward  Stepping  (6) 


2.  Stepping  Upward  Algorithm 

Figure  44  through  48  are  five  frames  from  the  kinematic  model  simulation.  These 
five  frames  show  one  step  cycle  of  the  Dynaman  while  stepping  upward. 


Figure  44:  Upward  Stepping  (1) 
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Figure  45:  Upward  Stepping  (2) 


Figure  46:  Upward  Stepping  (3) 


Figure  47:  Upward  Stepping  (4) 


Figure  48:  Upward  Stepping  (5) 


VI.  SUMMARY  AND  CONCLUSIONS 


A.  SUMMARY 

The  increasing  demand  for  realistic  3D  Virtual  Environments  requires  more 
research  on  modeling  human  motion.  There  exist  two  main  approaches  to  this  problem: 
kinematic  and  dynamic  modeling.  A  detailed  review  of  previous  work  on  kinematic 
modeling,  dynamic  modeling  and  control  of  graphical  representation  of  human  body 
motion  is  presented  in  this  thesis.  Kinematics  uses  only  the  geometric  constraints  of  the 
model  which  results  in  less  complicated  models.  However,  for  some  situations,  kinematic 
modeling  may  not  be  sufficient  to  simulate  human  motion  realistically.  Dynamics 
introduces  additional  properties  of  human  limb  segments  to  the  simulation,  such  as  mass 
and  moment  of  inertia.  These  additional  properties  allow  a  more  realistic  simulation.  The 
cost  of  this  realism  is  an  increase  in  computational  complexity.  The  goal  of  the  research  of 
this  thesis  is  to  develop  a  realistic  looking  human  model,  while  considering  the  complexity 
of  the  simulation  according  to  the  response  time  based  on  the  computational  power  of 
current  graphics  hardware. 

To  be  able  to  create  a  more  realistic,  real  time,  and  computationally  efficient  human 
model,  a  simple  dynamic  model  which  was  proposed,  but  not  fully  developed  in 
[MCGH79]  was  implemented.  The  model  is  a  two  link  planar  inverted  pendulum  which  is 
made  of  a  rigid  body  with  mass  and  a  supporting  massless  leg.  Two  different  methods, 
generalized  coordinates  with  Lagrangian,  and  the  Newton-Euler,  were  used  to  derive  the 
dynamic  equations  of  the  simulations.  The  implementation  results  verified  models  against 
each  other  and  related  models  in  the  literature  [GUBI74].  The  problem  of  determining  the 
gain  parameters  for  limb  segment  control  torques  is  also  investigated.  Since  the  degree  of 
the  characteristic  equation  is  four,  an  experimental  solution  is  presented,  instead  of  an 
analytic  one.  Another  result  is  the  difference  between  the  running  times  of  the  simulations. 
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The  simulation  which  uses  the  Newton-Euler  method  takes  20%  longer  than  the 
Lagrangian  version,  because  of  matrix  inversions.  It  is  also  observed  that  the  Lagrangian 
differential  equations  are  simpler  to  use  in  deriving  gain  values  for  the  system  in 
comparison  to  the  Newton-Euler  version  of  the  problem.  However,  the  Lagrangian  method 
would  be  hard  to  use  in  a  more  complex  model  with  higher  degrees  of  freedom.  For  such 
cases,  the  recursive  Articulated  Body  algorithm  [MCMI95]  which  has  O(n)  complexity 
needs  to  be  used. 

On  the  other  hand,  a  detailed  kinematic  human  model  which  contains  14  body  parts 
was  also  developed.  The  simulation  is  based  on  joints  angle  instead  of  the  Euler  angles 

which  used  in  previous  work.  IRIS  Performer™  API,  which  interfaces  to  both  IRIS  GL™ 

and  OpenGL.  IRIS  Performer™  and  provides  a  hierarchy  among  Dynamic  Coordinate 
Systems  (DCS),  was  used  to  implement  articulated  chain  structures  easily. 

B.  CONCLUSION  AND  FUTURE  RESEARCH 

This  thesis  covers  the  basics  for  simplified  dynamic  human  motion  simulation.  The 
model  which  is  introduced  in  this  thesis  has  only  the  torso  with  mass.  An  advanced  version 
of  this  simulation  may  consider  leg  mass  as  well.  The  existing  model  is  planar.  Another  step 
can  be  to  investigate  a  3D  version  of  the  model.  The  “Rigid-body”  class  could  continue  to 
be  used  for  this  purpose  since  provides  support  for  simulation  of  body  parts  in  3D. 
Multilink  more  complex  dynamic  models  also  need  to  be  investigated  by  considering 
different  solution  methods  such  as  Newton-Euler,  iterative  use  of  inverse  dynamics 
[KOOZ83],  and  the  Articulated  Body  Algorithm  [MCMI95].  Accuracy  and  time  response 
of  the  system  should  be  taken  into  account,  while  the  complexity  of  the  model  is  increased. 

Another  issue  is  to  integrate  the  existing  kinematic  and  dynamic  models.  This 
integration  may  result  a  more  realistic  human  model  than  the  developed  kinematic  model. 
It  will  be  computationally  cheaper  than  a  14  link  human  dynamic  model. 
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This  thesis  investigated  posture  control  of  a  human  model.  Stepping  is  another 
important  subject  in  human  motion  simulation.  An  efficient  automatic  stepping  algorithm 
needs  to  be  to  investigated  for  a  dynamically  simulated  human  model.  Active  (dynamic) 
balancing  should  be  considered  while  studying  automatic  stepping  control.  Advance 
features  in  further  research  in  stepping  is  speed  and  direction  control,  and  rough  terrain 
locomotion  for  the  walking  human  model.  For  direction  and  speed  control 
implementations,  user-computer  interaction  will  become  important.  A  speech  recognition 
interface  could  be  taken  in  the  consideration  as  a  realistic  solution  [DEVI96], 

VRML  is  a  fast  growing  3D  modeling  language.  It  is  possible  to  execute  the  needed 
computation  by  using  Java  Classes  which  are  integrated  to  VRML  nodes.  Implementing  the 
simulations  by  using  VRML  and  Java  Scripting  would  provide  platform  independency 
which  would  allow  the  simulations  runable  in  other  than  graphic  workstation 
environments.  The  author  hopes  that  the  work  of  this  thesis  will  provide  a  foundation  for 
continuing  research  in  some  of  the  above  topics. 
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APPENDIX  A:  DYNAMIC  SIMULATION  SOFTWARE 


COMMON  FILES  IN  ALL  DYNAMIC  SIMULATIONS 

********************************************************************* 

;;  File  :  robot-kinematics . cl 

;;  Author  :  Dr.  R.  McGhee 

;  ;  :  Naval  Postgraduate  School 

;  ;  :  Monterey,  CA  93  943 

; ;  Summary  :  This  file  contains  various  matrix  and  vector  operation 

;  ;  :  functions . 

. .  ********************************************************************* 

{defun  transpose  (matrix)  ;A  matrix  is  a  list  of  row  vectors. 

{cond  ((null  (cdr  matrix))  (mapcar  ’list  (car  matrix))) 

(t  (mapcar  ’cons  (car  matrix)  (transpose  (cdr  matrix)))))) 

{defun  dot-product  (vector-1  vector-2)  ;A  vector  is  a  list  of  numerical  atoms 
(apply  '+  (mapcar  '*  vector-1  vector-2))) 

(defun  vector-magnitude  (vector)  (sqrt  (dot-product  vector  vector) ) ) 

(defun  post-multiply  (matrix  vector) 

(cond  {(null  (rest  matrix))  (list  (dot-product  (first  matrix)  vector))) 

(t  (cons  (dot-product  (first  matrix)  vector) 

(post-multiply  (rest  matrix)  vector) ) ) ) ) 

(defun  pre-multiply  (vector  matrix) 

(post-multiply  (transpose  matrix)  vector) ) 

(defun  matrix-multiply  (A  B)  ;A  and  B  are  conformable  matrices. 

(cond  ( (null  (cdr  A) )  (list  (pre-multiply  (car  A)  B) ) ) 

(t  (cons  (pre-multiply  (car  A)  B)  (matrix-multiply  (cdr  A)  B) ) ) ) ) 

(defun  chain-multiply  (L)  ;L  is  a  list  of  names  of  conformable  matrices 

(cond  ((null  (cddr  L) )  (matrix-multiply  (eval  (car  L) )  (eval  (cadr  L) ) ) ) 

(t  (matrix-multiply  (eval  (car  L) )  (chain-multiply  (cdr  L) ) ) ) ) ) 

(defun  cycle-left  (matrix)  (mapcar  ’row-cycle-left  matrix)) 

{defun  row-cycle-left  (row)  (append  (cdr  row)  (list  (car  row)))) 

(defun  cycle-up  (matrix)  (append  (cdr  matrix)  (list  (car  matrix) ) ) ) 

(defun  unit-vector  (one-column  length)  ;Column  count  starts  at  1. 

(do  ( (n  length  (1-  n) ) 

(vector  nil  (cons  (cond  ( (=  one-column  n)  1)  (t  0))  vector))) 

( (zerop  n)  vector))) 


(defun  unit-matrix  (size) 

(do  ( (row- number  size  (1-  row-number) ) 
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(I  nil  (cons  (unit-vector  row-number  size)  I))) 

{ (zerop  row-number)  I))) 

(defun  concat-matrix  (A  B)  ;A  and  B  are  matrices  with  equal  number  of  rows, 
(cond  ( (null  A)  B) 

(t  (cons  (append  (car  A)  (car  B) )  (concat-matrix  (cdr  A) 

( cdr  B) ) ) ) ) ) 


(defun  augment  (matrix) 

(concat-matrix  matrix  (unit-matrix  (length  matrix)))) 

(defun  norma lize-row  (row)  ( scalar-multiply  (/  1.0  (car  row))  row)} 


(defun  scalar-multiply  (scalar  vector) 

(cond  ( (null  vector)  nil) 

(t  (cons  (*  scalar  (car  vector)) 

(scalar-multiply  scalar  (cdr  vector)))))) 


(defun  solve-f irst-column  (matrix)  .-Reduces  first  column  to  (10  ...  0). 

(do*  ( (remaining-row-list  matrix  (rest  remaining-row-list) ) 

(first-row  (normalize-row  (first  matrix))) 

(answer  (list  first-row) 

(cons  (vector-add  (first  remaining-row-list) 

(scalar-multiply  (-  (caar  remaining-row-list) ) 
first-row) ) 


answer) ) ) 

(  (null  (rest  remaining-row-list) )  (reverse  answer) ) ) ) 


(defun  vector-add  (vector-1  vector-2)  (mapcar  ’+  vector-1  vector-2)) 


(defun  vector-subtract  (vector-1  vector-2)  (mapcar  vector-1  vector-2)) 


(defun  first-square  (matrix)  ; Returns  leftmost  square  matrix  from  argument, 
(do  ((size  (length  matrix)) 

(remainder  matrix  (rest  remainder) ) 

(answer  nil  (cons  (firstn  size  (first  remainder))  answer))) 

( (null  remainder)  (reverse  answer) ) ) ) 


(defun  firstn  (n  list) 

(cond  ({zerop  n)  nil) 

(t  (cons  (first  list)  (firstn  (1-  n)  (rest  list)))))) 

(defun  max-car-f irstn  (n  list) 

(append  (max-car-f irst  (firstn  n  list) )  (nthcdr  n  list) ) ) 

(defun  matrix- inverse  (M) 

(do  ((Ml  (max-car-f irst  (augment  M) ) 

(cond  ( (null  Ml)  nil)  ; Abort  for  singular  matrix. 

(t  (max-car-f irstn  n  (cycle-left  (cycle-up  Ml)))))) 
(n  (1-  (length  M) )  (1-  n) ) ) 

((or  (minusp  n)  (null  Ml))  (cond  ((null  Ml)  nil)  (t  (first-square  Ml)))) 
(setq  Ml  (cond  ((zerop  (caar  Ml) )  nil)  (t  (solve-f irst-column  Ml) ) ) ) ) ) 

(defun  max-car-first  (L)  ;L  is  a  list  of  lists.  This  function  finds  list  with 

(cond- ((null  (cdr  L)  )  L)  .-largest  car  and  moves  it  to  head  of  list  of  lists. 
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(t  (if  (>  (abs  (caar  L) )  (abs  (caar  (max-car-first  (cdr  L) ) ) ) )  L 
(append  (max-car-first  (cdr  L) )  (list  (car  L) ) ) ) ) ) ) 

(defun  dh-matrix  (cosrotate  sinrotate  costwist  sintwist  length  translate) 
(list  (list  cosrotate  (-  (*  costwist  sinrotate)) 

(*  sintwist  sinrotate)  (*  length  cosrotate) ) 

(list  sinrotate  (*  costwist  cosrotate) 

(-  (*  sintwist  cosrotate))  (*  length  sinrotate)) 

(list  0.  sintwist  costwist  translate)  (list  0.  0.  0.  1.))) 

(defun  homogeneous-transf orm  (azimuth  elevation  roll  x  y  z) 

(let  ( (spsi  (sin  azimuth))  (cpsi  (cos  azimuth))  (sth  (sin  elevation)) 

( cth  (cos  elevation))  (sphi  (sin  roll))  (cphi  (cos  roll))) 

(list  (list  (*  cpsi  cth)  (-  (*  cpsi  sth  sphi)  (*  spsi  cphi)) 

(  +  {*  cpsi  sth  cphi)  (*  spsi  sphi))  x) 

(list  (*  spsi  cth)  (+  (*  cpsi  cphi)  (*  spsi  sth  sphi)) 

(-  (*  spsi  sth  cphi)  (*  cpsi  sphi))  y) 

(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi)  z) 

(list  0.  0.  0.  1. ) ) ) ) 

(defun  inverse-H  (H)  ;H  is  a  4x4  homogeneous  transformation  matrix, 

(let*  ( (minus-P  (list  (-  (fourth  (first  H) ) ) 

(-  (fourth  (second  H) ) ) 

(-  (fourth  (third  H) ) ) ) ) 

(inverse-R  (transpose  (first-square  (reverse  (rest  (reverse  H) ) ) ) ) ) 
(inverse-P  (post-multiply  inverse-R  minus-P))) 

(append  (concat-matrix  inverse-R  (transpose  (list  inverse-P))) 

(list  (list  0001))))) 

(defun  rotation-matrix  (azimuth  elevation  roll) 

(let  ({spsi  (sin  azimuth))  (cpsi  (cos  azimuth))  (sth  (sin  elevation)) 

(cth  (cos  elevation))  (sphi  (sin  roll))  (cphi  (cos  roll))) 

(list  (list  (*  cpsi  cth)  (-  {*  cpsi  sth  sphi)  (*  spsi  cphi)) 

(+  (*  cpsi  sth  cphi)  (*  spsi  sphi))) 

(list  (*  spsi  cth)  (+  {*  cpsi  cphi)  (*  spsi  sth  sphi) ) 

(-  (*  spsi  sth  cphi)  (*  cpsi  sphi))) 

(list  (-  sth)  (*  cth  sphi)  (*  cth  cphi))))) 

(defun  body-rate- to-euler-rate-matrix  (azimuth  elevation  roll) 

(let  ((sth  (sin  elevation))  (cth  (cos  elevation))  (tth  (tan  elevation)) 
(sphi  (sin  roll))  (cphi  (cos  roll))) 

(list  (list  1  ( *  tth  sphi)  (*  tth  cphi)) 

(list  0  cphi  (-  sphi)) 

(list  0  (/  sphi  cth)  (/  cphi  cth))))) 
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;;  File  :  harmonic-equation . cl 

;  ;  Author  :  Dr .  R .  McGhee 

;  ;  :  Naval  Postgraduate  School 

;  ;  :  Monterey,  CA  93  943 

; ;  Summary  :  This  file  contains  functions  to  create  a  window  and  execute 

;  ;  :  drawing  operations  on  the  window. 

.  .  *★******★★*★★*★★****★****■*★*★★★****★★****★★****’•*•***■*■★*•*•*■*■*••****★★**'*** 

(require  :xcw) 

(cw: initialize-common-windows ) 

(defun  make-window  () 

(cw: make-window-stream  : borders  5 

: left  10 
zbottom  280 
:width  600 
: height  600 

: title  "Harmonic  Equation" 

:activate-p  t) )  ;Make  window  visible. 


(defun  scale-point-coordinates  (x-y-list  enlargement- factor) 
(let  ( (x  (first  x-y-list))  (y  (second  x-y-list))) 

(list  (  +  50  (round  (*  enlargement- factor  x) ) ) 

(+-  225  (round  (*  enlargement-factor  y)  )  )  )  )  ) 


(defun  draw-coordinate-axes  (window) 

(cw: draw- line  window  (cw: make-position 

( cw : make-position 
: brush-width  2) 

(cw: draw-line  window  ( cw:make-position 

( cw: make-position 
: brush-width  2)) 


: x  20  : y  225) 

: x  570  : y  225) 

: x  50  : y  20) 

: x  50  :y  560) 


(defun  draw- line- in-window  (window  enlargement- factor  start  end) 

(let  ((scaled-start  (scale-point-coordinates  start  enlargement- factor ) ) 
(scaled-end  (scale-point-coordinates  end  enlargement- f actor ) ) ) 

(cw: draw- line  window 

(cw:make-position  :x  (first  scaled-start)  :y  (second  scaled-start)) 
(cw:make-position  :x  (first  scaled-end)  :y  (second  scaled-end))))) 
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SINGLE  LINK  INVERTED  PENDULUM  SIMULATION  WITH  NEWTfVM -EULER 


File 

Author 


Modified  by 
Summary 


euler-angle-rigid-body . cl 
Dr .  R .  McGhee 
Naval  Postgraduate  School 
Monterey,  CA  93943 
Mehmet  Bediz 

This  file  contains  rigid  body  class  which  is  implemented 
by  Dr.  R.  McGhee.  The  function  "test-rigid-body- f orces-and- 
torques-three-with-M"  and  the  other  functions  called  by  this 
function  are  implemented  by  Mehmet  Bediz. 


(defclass  rigid-body 

0 

( (posture  ;The  vector  (xe  ye  ze  phi  theta  psi) . 

rinitform  ' (0  0  0  0  0  0) 

:initarg  :posture 
.■accessor  posture) 

(posture-rate  ;The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 
rinitarg  : posture-rate 
: accessor  posture-rate) 

(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates, 

rinitform  1 (0  0  0  000) 

rinitarg  : velocity 
: accessor  velocity) 

(velocity-growth-rate  ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 

( f orces-and-torques  ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

; : ini t form  (list  0  0  (-  *gravity*)  000) 

: initf orm  (list  000000) 

: accessor  forces-and- torques ) 

(moments-of-inertia  ;The  vector  (lx  Iy  Iz)  in  principal  axis  coordinates, 
rinitform  '(1  900  1) 
rinitarg  : moments-of-inertia 
r accessor  moments-of-inertia) 

(mass 

rinitform  100 
rinitarg  :mass 
r accessor  mass) 

(node-list  ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  0  1) . 
rinitform  ’((000  1)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1) (0-5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1) (-0.5  0.5  0.5  1) (0.5  0.5  0.5  1) 

(0.5  0.5  -0.5  1 )  (-0.5  0.5  -0.5  1)  (0  0  0  1) 

(0031)) 

rinitarg  mode-list 
r accessor  node-list) 

(polygon-list 

rinitform  1 ( ( 1  5  8  4 ) ( 5  6  7  8 ) ( 6  2  3  7 ) (2  1  4  3 ) ( 9  10)) 
rinitarg  rpolygon-list 
r accessor  polygon-list) 

( transf ormed-node-list  ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list, 
rinitform  '((0001)  (-2  -2  0  1)  (2  -2  0  1) (2  -2  -30  1)(~2  -2  -30  1) 

(-2  2  0  1)  (2  2  0  1) (2  2  -30  l)(-2  2  -30  1)) 
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: accessor  transf ormed-node-list) 

(H-matrix 

:initform  (unit-matrix  4) 

: accessor  H-matrix) 

(time-stamp 

: accessor  time-stamp) ) ) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  (transf ormed-node-list  body)  (node-list  body) ) 

( setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 
(setf  (posture-rate  body)  (earth-velocity  body)) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 

(setf  ( third ( fourth  ( transf ormed-node-list  body) ) ) (-  z) ) 

(setf  (third(fifth  ( transf ormed-node-list  body)))  (-  z) ) 

(setf  ( third(eighth  ( transf ormed-node-list  body)))  (-  z) ) 

(setf  (third(ninth  (transf ormed-node-list  body)))  (-  z) ) 

(transf ormed-node-list  body) ) ) 


(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 


(defun  reverse-transform  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  ( (body  rigid-body)  azimuth  elevation  roll  x  y  z) 
(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth)) 

(setf  (H-matrix  body) 

(homogeneous- transform  azimuth  elevation  roll  x  y  z) ) 
(transform-node-list  body)) 


(defmethod  get-delta-t  ((body  rigid-body))  0.02) 

;  (let*  ( (new- time  (get-internal-real-time) ) 

;  (delta-t  (/  (-  new-time  (time-stamp  body))  1000))) 

;  (setf  (time- stamp  body)  new- time) 

;  delta-t) ) 


(defmethod  update-rigid-body  ((body  rigid-body))  ;Euler  integration. 

(let*  ({delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 
(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous-transform  (sixth  (posture  body) ) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body) )  (third  (posture  body) ) ) ) 

(transform-node-list  body) 

(update-velocity-growth-rate  body) ) ) 


(defmethod  update-velocity-growth-rate  ( (body  rigid-body) ) 

(setf  {velocity-growth-rate  body)  ; Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N  u  v  w  p  q  r  lx  Iy  Iz)  ; Declares  local  variables. 

( values-list  ;Values  assigned. 

( append 

( forces-and-torques  body)  (velocity  body)  (moments-of-inertia  body))) 
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(list  (+  (*  v  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 

(*  ^gravity*  (first  (third  (H-matrix  body))))) 

(  +  (*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

(*  *gravity*  (second  (third  (H-matrix  body))))) 

(+  (*  u  q)  (*  -1  v  p)  (/  Fz  (mass  body)) 

(*  * gravity*  (third  (third  (H-matrix  body))))) 

(/  (+  (*  (-  Iy  I z )  q  r)  L)  lx) 

(/  (+  (*  (-  Iz  lx)  r  p)  M)  Iy) 

(/  (  +  (*  (-  lx  Iy)  p  q)  N)  Iz) ) ) ) )  ;Value  returned. 

(defmethod  update-velocity  ((body  rigid-body)  delta-t)  ;Euler  integration. 

(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  ( (body  rigid-body)  delta-t)  ; Euler  integration. 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body) ) ) ) ) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 

(setf  ( transf ormed-node-list  body) 

(mapcar  #’ (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 

(node-list  body) ) ) ) 

(defconstant  ^gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  ( (linear-velocity  (firstn  3  (velocity  body) ) ) 

(rotational-velocity  (cdddr  (velocity  body) ) ) 

(posture  (posture  body) ) 

(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture))) 

(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity) ) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 

(append  linear-earth- velocity  rotational-earth-velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  'rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make-instance  ’strobe-camera)) 

(move  camera- 1  (-  (/  pi  2))  0  00300) 

(take-picture  camera- 1  airplane-1) 

(dotimes  (i  20  'done)  (update-rigid-body  airplane-1)) 

(take-picture  camera-1  airplane-1)) 
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/tfttrrirt/ftfiit 


t  t  r  f  r  r  t 


r  t  t  f  t  r  r  r  t 


t  t  /  /  /  /  /  / 


-  Inverted  Pendulum  (fixed  leg  length) 

-  state  vector  x  =  (theta  theta-dot) 

-  state  vector  u  =  (M) 

-  control  torque  at  the  pivot  (M) 

-  Newton-Euler  Method 

3x3  matrix  inversion  to  calculate  forces  and  moments 
RIGID  Body  class  to  compute  accelerations 

-  Heun  Integration 


(defmethod  f orces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 

(let*  ( (forces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0))  ;Since  planar 
(forces-body  (post-multiply  R-matrix  forces-earth))) 


forces-body) ) 

(defmethod  update-forces-and-torques-three-with~M  ( (body  rigid-body)  M) 

(let*  ( ( equations-solution-vector  ( solve- three-equation-system-with-M 

body  M) ) 

(Fx-earth  (second  equations-solution-vector)) 

(Fz-earth  (third  equations-solution-vector) ) 

(F-body  ( f orces-earth-to-body  Fx-earth  0  Fz-earth 

(fifth  (posture  body))))) 

(setf  f orces-and-torques  (list  (first  F-body) 

0 

(third  F-body) 

0 

(*  (first  equations-solution-vector) 
(second  (moments-of-inertia  body) ) ) 

0)  )  )  ) 


(defun  solve-three-equation-system-with-M  (body  M) 
(setf  m  (mass  body) ) 

(setf  1  3) 

(setf  I  (second  (moments-of-inertia  body) ) ) 

(setf  theta  (fifth  (posture  body))) 

(setf  theta-dot  (fifth  (velocity  body))) 

(setf  g  *gravity*) 


(post-multiply  (matrix- inverse  (list  (list  I  ( i 


-1  1  (cos  theta) ) 

( *  1  (sin  theta) ) ) 

(list  (*  m  1  (cos  theta))  1  0  ) 

(list  (*  m  1  (sin  theta)  )  0  -1  )  )  ) 

(list  M 

(*  (sqr  theta-dot)  m  1  (sin  theta)) 

(-  (*  m  g)  (*  (sqr  theta-dot)  m  1  (cos  theta)))))) 
(defun  compute-M  (body  K-theta  K-theta-dot) 

(setf  theta  (fifth  (posture  body))) 

(setf  theta-dot  (fifth  (velocity  body) ) ) 
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{+  (*  -1  K-theta  theta)  (*  -1  K-theta-dot  theta-dot))) 


(defun  test-rigid-body-f orces-and-torques-three-with-M  (K-theta  K-theta-dot) 
(setf  invert ed-pendulum  (make- instance  'rigid-body)) 

(initialize  inverted-pendulum) 

(setf  camera-1  (make-instance  ’strobe-camera)) 

(move  camera-1  (deg-to-rad  (-  90 ) ) (deg-to-rad  0) (deg-to-rad  0)  0  10  0) 

(move  inverted-pendulum  0-10000) 

; (take-picture  camera-1  inverted-pendulum) 

(setf  graph- 1 -window  (make-window-graph- 1 ) ) 

(draw-coordinate-axes  graph-l-window) 

(setf  old-theta  0.5) 

(do  (  (i  0  (+  i  1)  )  ) 

( (  >  i  1000)  ' end) 

(setf  ( forces-and- torques  inverted-pendulum) 

( update- f orces- and- torques -three-with-M  inverted-pendulum 
(compute-M  inverted-pendulum  K-theta  K-theta-dot))) 
(update-rigid-body  inverted-pendulum) 

(cwrclear  (camera-window  camera-1)) 

(take-picture  camera-1  inverted-pendulum) 

(draw- line- in-window  graph-l-window  80 

(list 

(*  (get-delta-t  inverted-pendulum)  (-  i  1) ) 
(*  -1  old-theta)) 

(list 

(*  (get-delta-t  inverted-pendulum)  i) 

(*  -1  (fifth  (posture  inverted-pendulum) ) ) ) ) 

(setf  old- theta  (fifth  (posture  inverted-pendulum))))) 

(defun  sqr(x) 

(*  xx)) 

(defun  make-window-graph-1  () 

(cw: make- window- stream  : borders  5 

: left  0 
:bottom  550 
: width  450 
:height  450 

: title  "Body  Attitude  (theta)  NEWTON- EULER" 

:activate-p  t) )  ;Make  window  visible. 


.  .  *******★********★*★*************★******★*★********★*■★★•**★**•**★★***★★* 

;;  File  :  load-euler- files . cl 

; ;  Author  :  Mehmet  Bediz 

;  ;  :  Naval  Postgraduate  School 

; ;  :  Monterey,  CA  93943 

; ;  Summary  :  This  file  contains  functions  to  load  files  and  to  test 

;;  :  the  simulations. 

.  .  t**************-****************************************************** 


;  Load  files 

i  r  t  /  r  f  r  t  /  f  f  r  /  r  t  i  r  //  i  r  t  /  r  //  r  //  /  r  /  / 

(load  "harmonic-equation . cl " ) 

(load  M robot-kinematics . cl M ) 

( load  " euler-angle-rigid-body . cl " ) 

(load  " strobe-camera . cl " ) 

f  /  /  /  /  /  r  /  /  /  /  /  /  /  /  /  r  f  /  /  /  /  /  f  /  }  /  /  /  /  /  /  / 

;  Tests 

r  t  t  r  t  t  t  t  it  t  t  t  /  t  t  t  t  r  i  t  i  i  i  i  i  i  t  ii  t  t  t 

r  r  t  t  r  /  t  t  r  t  r  r  r  f  /  t  t  i  r  r  t  t  /  r  i  f  /  i  t  r  i  r  r  /  /  /  t  >  r  r  i  r  r  r  i  r  r  i  t  r  t  r  r  t  f  t  /  r  /  /  t  t  r  i  r  r  /  t  r  r  r  t 


{defun  graph_l  () 

(solve-robot-attitude  0.01  1.0  10000  2000  109660  20000)) 
(defun  graph_2  () 

( solve-robot-altitude-z_zO  0.01  1.0  10000  2000  109660  20000)) 
{defun  graph_3  () 

{ solve-robot-altitude-z  0.01  1.0  10000  2000  109660  20000)) 

(defun  draw-converted-pendulum- test-real-parameters  () 

(draw-converted-pendulum  0.01  2.0  10000  2000  109660  20000)) 

(defun  draw-converted-pendulum- test- zero -gain  () 

(draw-converted-pendulum  0.012.0  0  0  0  0)) 
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File  :  strobe-camera . cl 

Author  :  Dr.  R.  McGhee 

:  Naval  Postgraduate  School 
:  Monterey,  CA  93943 

Summary  :  This  file  contains  strobe-camera  class  definitions. 

********************************************************************* 


(require  :xcw) 

(cw: initialize-common-windows ) 

(defclass  strobe-camera  (rigid-body) 

( (focal-length 

: accessor  focal-length 
:initform  6) 

( camera-window 
: accessor  camera-window 

:initform  { cw : make-window- stream  rborders  5 

: left  500 
ibottom  500 
: width  650 
: height  650 

: title  "strobe-camera- image" 

:activate-p  t) ) 

(H-matrix 

:initform  (homogeneous- transform  .3  -.3  0  -300  -90  -90)) 

( inverse-H-matrix 
: accessor  inverse-H-matrix 

rinitform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 

( enlargement- factor 
: accessor  enlargement- factor 
: initf orm  30 ) ) ) 

(defmethod  move  ( (camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous- transform  azimuth  elevation  roll  x  y  z 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 

(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 
(post-multiply  (inverse-H-matrix  camera)  node-location) ) 
(transformed-node-list  body) ) ) ) 

(dolist  (polygon  (polygon-list  body) ) 

( clip-and-draw-polygon  camera  polygon  camera-space-node-list) ) ) ) 

(defmethod  clip-and-draw-polygon 

( (camera  strobe-camera)  polygon  node-coord-list) 

(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 

(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes ) ) 

(to-point  (nth  (first  remaining-nodes)  node-coord-list) 

(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 

( (null  to-point) 

(draw-clipped-projection  camera  from-point  initial-point) ) 
(draw-clipped-pro jection  camera  from-point  to-point) ) ) 
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(defmethod  draw-clipped-projection  ( (camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 

( (<=  (first  from-point)  (focal-length  camera) ) 

(draw- line- in-camera-window  camera 

(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transf orm  camera  to-point) ) ) 

( {<=  (first  to-point)  (focal-length  camera)) 

(draw- line- in- camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective-transform  camera  (to-clip  camera  from-point  to-point)))) 
(t  (draw- line- in- camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective-transf orm  camera  to-point) ) ) ) ) 


(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 

(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 

(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point) ) ) ) 

(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1) ) ) 


(defmethod  to-clip  ( (camera  strobe-camera)  from-point  to-point) 
(from-clip  camera  to-point  from-point)) 


(defmethod  draw- line- in-camera-window  ( (camera  strobe- camera)  start  end) 

(cw: draw- line  (camera-window  camera) 

(cw: make-position  :x  (  +  150  (first  start))  :y  (+  150  (second 

start) ) ) 

(cw:make-position  :x  (+  150  (first  end) )  :y  (+  150  (second 

end) ) ) 

: brush-width  0)) 


(defmethod  perspective-transf orm  ( (camera  strobe-camera)  point-in-camera-space) 
(let*  ((enlargement-factor  (enlargement- factor  camera)) 

(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space) )  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space) )  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement- factor  (/  (*  focal-length  y)  x) ) ) 
150)  ; to  right  in  camera  window 

(  +  150  (round  (*  enlargement- factor  (/  (*  focal-length  (-  z) )  x) ) 

) ) ) ) )  ;up  in  camera  window 

(defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body. 

(setf  robot-leg  (make-instance  'rigid-body)) 

(initialize  robot-leg) 

(set-transformed-node-list-z  robot-leg  z) 

(set-transf ormed-node-list-theta  robot-leg  theta) 

(setf  camera-1  (make-instance  'strobe-camera)) 

(move  .camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -30  0  0) 
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(take-picture  camera-1  robot-leg)) 


(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 
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TWO  LINK  INVERTED  PENDULUM  SIMULATION  WITH  LAGRANGIAN 


/  / 

;;  File  :  euler-angle-rigid-body . cl 

;;  Author  :  Dr.  R.  McGhee 

;  ;  :  Naval  Postgraduate  School 

; ;  :  Monterey,  CA  93943 

;  ;  Modified  by  :  Mehmet  Bediz 

; ;  Summary  :  This  file  contains  rigid  body  class  which  is  implemented 

;;  :  by  Dr .  R.  McGhee.  The  function  w draw- inverted-pendulum" 

;  ;  :  and  the  other  functions  called  by  this 

;;  :  function  are  implemented  by  Mehmet  Bediz. 

/  / 

(defclass  rigid-body 

0 

{ (posture  ; The  vector  (xe  ye  ze  phi  theta  psi) . 

:initform  ’(000000) 
zinitarg  : posture 
: accessor  posture) 

(posture-rate  ;The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 
:initarg  : posture-rate 
: accessor  posture-rate) 

(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates, 

zinitform  ’(000  000) 

:initarg  : velocity 
: accessor  velocity) 

(velocity-growth-rate  ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 

( forces-and- torques  ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates, 
zinitform  (list  000000) 

: accessor  f orces-and-torques ) 

(moments-of -inertia  ;The  vector  (lx  Iy  Iz)  in  principal  axis  coordinates. 

: initform  1 ( 1  100  1) 
zinitarg  : moments -of -inertia 
: accessor  moments-of-inertia) 

(mass 

zinitform  100 
zinitarg  zmass 
z accessor  mass) 

(node-list  ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  0  1) . 
zinitform  ’((0  001)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1)(0.5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1)  (-0.5  0.5  0.5  1)  (0.5  0.5  0.5  1) 

(0.5  0.5  -0.5  1 ) (-0.5  0.5  -0.5  1) (0  0  0  1)(0  0  3  1)) 

zinitarg  znode-list 
zaccessor  node-list) 

(polygon-list 

zinitform  1 ( (1  5  8  4) (5  6  7  8) (6  2  3  7) (2  1  4  3) (9  10)) 
zinitarg  : polygon-list 
zaccessor  polygon-list) 

( transf ormed-node-list  ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list, 
zinitform  ’((0001)  (-2  -2  0  1)  (2  -2  0  1) (2  -2  -30  1) (-2  -2  -30  1) 

(-2  2  0  1)  (2  2  0  1) (2  2  -30  1) (-2  2  -30  1)) 

zaccessor  transf ormed-node-list ) 

(H-matrix 
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:initform  (unit-matrix  4) 

: accessor  H-matrix) 

(time- stamp 

: accessor  t ime- stamp) ) ) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  ( transformed-node-list  body)  (node-list  body) ) 

(setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 

(setf  (posture-rate  body)  (earth-velocity  body)) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 

(setf  { third ( fourth  (transformed-node-list  body)))(-  z) ) 

(setf  (third(fifth  (transformed-node-list  body)))  (-  z) ) 

(setf  (third (eighth  (transformed-node-list  body)))  (-  z) ) 

(setf  (third(ninth  (transformed-node-list  body)))  (-  z) ) 
(transformed-node-list  body) ) ) 

(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 

(defun  reverse-transf orm  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  ( (body  rigid-body)  azimuth  elevation  roll  x  y  z) 

(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth)) 

(setf  (H-matrix  body) 

(homogeneous- transform  azimuth  elevation  roll  x  y  z) ) 

( transf orm-node-list  body) ) 

(defmethod  get-delta-t  ((body  rigid-body))  0.005) 

(defmethod  f orces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 

(let*  ( ( f orces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0)) 

(forces-body  (post-multiply  R-matrix  f orces-earth) ) ) 

forces-body) ) 

(defmethod  update-rigid-body  ( (body  rigid-body) )  ; Euler  integration. 

(let*  ( (delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 
(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous -transform  (sixth  (posture  body) ) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body))  (third  (posture  body)))) 

(transform-node- list  body) 

(update-velocity-growth-rate  body) ) ) 

(defmethod  update-velocity-growth-rate  ((body  rigid-body)) 

(setf  (velocity-growth-rate  body)  ; Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N  u  v  w  p  q  r  lx  Iy  Iz)  ;Declares  local  variables. 
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;Values  assigned. 


(values-list 
(append 

( forces-and-torques  body)  (velocity  body)  (moments-of-inertia  body))) 
(list  (+  (*  v  r)  (*  -1  w  g)  (/  Fx  (mass  body)) 

(*  *gravity*  (first  (third  (H-matrix  body))))) 

(+  (*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

(*  ^gravity*  (second  (third  (H-matrix  body))))) 

(+  (*  u  q)  (*  -1  v  p)  (/  Fz  (mass  body)) 

(*  ^gravity*  (third  (third  (H-matrix  body))))) 

(/  (+  (*  (-  Iy  Iz)  q  r)  L)  lx) 

(/  (  +  (*  (-  Iz  lx)  r  p)  M)  Iy) 

(/  (+  (*  (-  lx  Iy)  p  q)  N)  Iz) ) ) ) )  ;Value  returned. 

(defmethod  update-velocity  ({body  rigid-body)  delta-t)  ;Euler  integration. 

(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  {(body  rigid-body)  delta-t)  ;Euler  integration. 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body) ) ) ) ) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 

(setf  ( transf ormed-node-list  body) 

(mapcar  #' (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 

(node-list  body) ) ) ) 

(defconstant  *gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  ( (linear-velocity  (firstn  3  (velocity  body) ) ) 

(rotational-velocity  (cdddr  (velocity  body) ) ) 

(posture  (posture  body) ) 

(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture))) 

(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity) ) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 

(append  linear-earth-velocity  rotational-earth-velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  ’rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make-instance  ’strobe-camera)) 

(move  camera-1  (-  (/  pi  2))  0  0  0  30  0  ) 

(take-picture  camera-1  airplane-1) 

(dotimes  (i  20  ’done)  (update-rigid-body  airplane-1)) 

(take-picture  camera-1  airplane-1)) 
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;  -  Two  Link  Inverted  Pendulum 

;  -  Generalized  Coordinates  with  Lagrangian  Method 

;  -  Massless  leg  and  a  single  rigid  body  (fixed  leg  length) 

;  -  State  vector  x  =  (theta-1  theta-l-dot  theta-2  theta-2-dot) 

;  -  State  vector  u  =  (M-l  M-2) 

;  -  control  torque  at  the  ankle  (M-l) 

;  -  control  torque  at  the  hip  (M-2) 

;  -  Heun  Integration 

;  -  By  Mehmet  Bediz 

( setf  1  0.5) 

(setf  r  3) 

(setf  time-step  0.01) 

(defun  euler-step-constant-leg  (state-vector  M-l  M-2) 

(mapcar  1 +  state-vector  ( scalar-multiply  time-step 

(derivative- state-vector 
state-vector  M-l  M-2)))) 

(defun  heun-step-constant-leg  (x  K-theta-1  K- theta-l-dot 

K-theta-2  K-theta-2-dot) 

(setf  M-l  (compute-M-1  x  K-theta-1  K- theta-l-dot 

K-theta-2  K-theta-2-dot ) ) 

(setf  M-2  (compute-M-2  x  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot ) ) 

(mapcar  ' +  x  (scalar-multiply  (*  time-step  .5) 

(vector-add  (derivative-state-vector  x  M-l  M-2) 

(derivative-state-vector 

(euler-step-constant-leg  x  M-l  M-2)  M-l  M-2))))) 

(defun  derivative-state-vector  (state-vector  M-l  M-2) 

(list  (second  state-vector) 

(compute-theta-l-double-dot  state-vector  M-l  M-2) 

(fourth  state-vector) 

(compute-theta-2-double-dot  state-vector  M-l  M-2))) 

(defun  compute-theta-l-double-dot  (state-vector  M-l  M-2)  ;  Eq  3.51 


(setf 

m 

100) 

(setf 

1 

0.5) 

(setf 

r 

3) 

(setf 

I 

100) 

(setf 

theta-1 

(first 

state-vector) ) 

(setf 

theta-l-dot 

(second 

state-vector) ) 

( setf 

theta-2 

(third 

state-vector) ) 

(setf 

theta-2-dot 

( fourth 

state-vector) ) 

(setf 

g 

32.2) 

(  / 

(+  (* 

1  1  m  m  r 

g  theta- 

•2) 

(* 

-1  m  g  r 

{ +  I  (*  m  1  1) )  theta- 1 

(* 

-1  (+  I  ( 

*  m  1  1)  ) 

M-l) 

(* 

(+  (*  1  m 

r)  I  {* 

mil))  M-2) 

) 

(*  -1  I  m  r  r) ) ) 
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(defun  compute- theta-2-double-dot  (state-vector  M-l  M-2)  ;  Eq  3.54 


( setf 

m 

100) 

(setf 

1 

0.5) 

(setf 

r 

3) 

(setf 

I 

100) 

(setf 

theta-1 

(first 

state-vector) ) 

(setf 

theta-1- 

-dot 

(second 

state-vector) ) 

(setf 

theta-2 

( third 

state-vector) ) 

(setf 

theta-2- 

-dot 

( fourth 

state-vector) ) 

(setf 

g  32 

-2) 

(  /  (+  (*  m  g  1  r  theta-2) 

(*  -1  m  g  1  r  theta-1) 

(*  -1  1  M-l) 

(*  (+  r  1)  M-2) 

) 

(*  r  I) 

) 

) 

(defun  compute-M-1  (state-vector  K-theta-1  K- theta-l-dot 

K-theta-2  K- theta-2-dot) 

(setf  theta-1  (first  state-vector)) 

(setf  theta-l-dot  (second  state-vector) ) 

{+  (*  -1  K-theta-1  theta-1) (*  -1  K- theta-l-dot  theta-l-dot))) 

(defun  compute-M-2  (state-vector  K-theta-1  K- theta-l-dot 

K-theta-2  K-theta-2-dot ) 

(setf  theta-2  (third  state-vector)) 

(setf  theta-2-dot  (fourth  state-vector)) 

(+  (*  -1  K-theta-2  theta-2) (*  -1  K-theta-2-dot  theta-2-dot) ) ) 

(defun  compute-y  (state-vector) 


(setf 

theta-1 

{ first 

state-vector) ) 

(setf 

theta-l-dot 

(second 

state-vector) )  , 

(setf 

theta-2 

(third 

state-vector) ) 

(setf 

theta- 2 -dot 

( fourth 

state-vector) ) 

(setf 

1  0.5) 

i 

j 

(setf 

r  3) 

<  +  (* 

r  (sin  theta-1) ) 

(* 

1  (sin  theta-2) 

)  )  ) 

defun  compute-2  (state- 

vector) 

(setf 

theta-1 

( first 

state-vector) ) 

(setf 

theta-l-dot 

(second 

state-vector) ) 

( setf 

theta-2 

(third 

state-vector) ) 

(setf 

theta-2-dot 

( fourth 

state-vector) ) 

(setf 

1  0.5) 

(setf 

r  3) 

1 

(  +  <* 

r  (cos  theta-1) 

) 

(* 

1  (cos  theta-2) 

)  )  ) 

(defun  draw- inverted-pendulum  (halt-time  K-theta-1  K- theta-l-dot 

K-theta-2  K-theta-2-dot ) 
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(setf  robot-body  (make-instance  ’rigid-body)) 
(initialize  robot-body) 

(setf  robot-leg  (make- instance  'rigid-body)) 
(initialize  robot-leg) 


(setf  camera-1  (make- instance  ’strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -10  0  0) 

(setf  graph- 1 -window  (make-window-graph-1)) 

( draw-coordinate-axes  graph- 1-window) 

(setf  graph- 2 -window  (make-window-graph-2)) 

(draw-coordinate-axes  graph- 2 -window) 

(do  (  (x  '(0.5  0  1.0  0)  (heun-step-constant-leg  x  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot) ) 

(old-x  ' (0.5  0  1.0  0)  x) 

(time  0  (+  time  1) ) ) 

( (>  time  halt-time)  ’done) 

(move  robot-body  (deg-to-rad  0) (deg-to-rad  0)  (third  x) 

0  (compute-y  x)  (*  -l(compute-z  x) ) ) 

(setf  ( transformed-node-list  robot-leg)  (list  (list  0001) 

(list  0  (*  r  (sin  (first  x) ) ) 

(*  -1  r  (cos  (first  x) ) )  1) 
(list  0001))) 

(setf  (polygon-list  robot-leg)  (list  (list  21))) 

(cw:clear  (camera-window  camera-1)) 

(take-picture  camera-1  robot-body) 

(take-picture  camera-1  robot-leg) 

(draw- line- in-window  graph- 1 -window  80  (list  (*  time- step  (-  time  1) ) 

( first  old-x) ) 

(list  (*  time-step  time) 

(first  x) ) ) 

(draw- line- in-window  graph- 2 -window  80  (list  (*  time-step  {-  time  1)) 

(third  old-x)) 

(list  (*  time-step  time) 

(third  x) ) ) ) ) 

(defun  make-window-graph- 1  () 

(cw: make-window- stream  : borders  5 

: left  0 
: bottom  50 
:width  450 
: height  450 

: title  "Leg  Angle  (theta-1)  LAGRANGIAN" 

:activate-p  t) )  ;Make  window  visible. 

(defun  make-window-graph- 2  () 

(cw: make-window- stream  : borders  5 

: left  0 


bottom  550 
width  450 
height  450 

title  "Body  Attitude  (theta-2)  LAGRANGIAN" 
activate-p  t) )  ;Make  window  visible. 


( 
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File  :  load-euler-f iles . cl 

Author  :  Mehmet  Bediz 

:  Naval  Postgraduate  School 
:  Monterey,  CA  93943 

Summary  :  This  file  contains  functions  to  load  files. 

********************************************************************** 


;  Load  files 


(load  "harmonic -equation. cl " ) 

(load  "robot-kinematics . cl " ) 

(load  " euler-angle-rigid-body . cl " ) 
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;  ;  File 
;  ;  Author 


Summary 


strobe-camera . cl 
Dr.  R.  McGhee 
Naval  Postgraduate  School 
Monterey,  CA  93943 

This  file  contains  strobe-camera  class  definitions. 


(require  :xcw) 

(cw:  initialize-common-windows ) 

(defclass  strobe-camera  (rigid-body) 

( ( focal-length 

: accessor  focal-length 
:initform  6) 

( camera-window 
: accessor  camera-window 

:initform  ( cw: make- window- stream  : borders  5 

: left  470 
rbottom  500 
: width  650 
: height  650 

: title  "Two  Link  Inverted  Pendulum 

(Constant  Length  Massless  Leg) 
LAGRANGIAN" 

:activate-p  t) ) 

(H-matrix 

:initform  (homogeneous- transform  .3  -.3  0  -300  -90  -90)) 

( inverse-H-matrix 
: accessor  inverse-H-matrix 

zinitform  (inverse-H  (homogeneous -transform  .3  -.3  0  -300  -90  -90))) 

( enlargement- factor 
: accessor  enlargement- factor 
: initform  30) ) ) 

(defmethod  move  ( (camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous -transform  azimuth  elevation  roll  x  y  z) ) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera)))) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 

(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 

(post-multiply  (inverse-H-matrix  camera)  node-location) ) 

( transf ormed-node-list  body) ) ) ) 

(dolist  (polygon  (polygon-list  body) ) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list) ) ) ) 

(defmethod  clip-and-draw-polygon 

( (camera  strobe- camera)  polygon  node-coord-list) 

(do*  ((initial-point  (nth  (first  polygon)  node-coord-list)) 

(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes ) ) 

(to-point  (nth  (first  remaining-nodes)  node-coord-list) 

(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))) 

(-(null  to-point) 
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(draw-clipped-projection  camera  from-point  initial-point) ) 
(draw-clipped-projection  camera  from-point  to-point) ) ) 

(defmethod  draw-clipped-projection  ( (camera  strobe-camera)  from-point  to-point) 
(cond  {(and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 

( (<=  (first  from-point)  (focal-length  camera)) 

{ draw- line- in- camera-window  camera 

(perspective-transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transf orm  camera  to-point) ) ) 

( (<=  (first  to-point)  (focal-length  camera)) 

( draw- 1 ine- in- camera-window  camera 

(perspective-transform  camera  from-point) 

(perspective- transform  camera  (to-clip  camera  from-point  to-point)))) 
(t  (draw- line- in-camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective-transf orm  camera  to-point) ) ) ) ) 

(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 

(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 

(+  (second  from-point) 

(*  scale-factor  (-  (second  to-point)  (second  from-point)))) 

(+  (third  from-point) 

(*  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ( (camera  strobe-camera)  from-point  to-point) 

(from-clip  camera  to-point  from-point)) 

(defmethod  draw-line-in-camera-window  { (camera  strobe- camera)  start  end) 

(cw: draw- line  (camera-window  camera) 

(cw: make-position  :x  (+  150  (first  start)) 

:y  (+  150  (second  start))) 

(cwimake-position  :x  (+  150  (first  end)) 

:y  (+  150  (second  end) ) ) 

: brush-width  0) ) 

(defmethod  perspective- transform  ( (camera  strobe-camera)  point-in~camera-space) 
(let*  ( (enlargement- factor  (enlargement- factor  camera)) 

(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-space))  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ?y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  bottom  of  camera 
(list  (+  (round  (*  enlargement- factor  (/  (*  focal-length  y)  x) ) ) 
150)  ; to  right  in  camera  window 

(  +  150  (round  (*  enlargement- factor  (/  {*  focal-length  (-  z) )  x) ) 

) ) ) ) )  ; up  in  camera  window 

{ defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body. 

(setf  robot-leg  (make- instance  'rigid-body)) 

(initialize  robot-leg) 

(set- transf ormed-node-list-z  robot-leg  z) 

(set-transf ormed-node- list- theta  robot-leg  theta) 
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(setf  camera-1  (make-instance  ’strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0) 
(take-picture  camera-1  robot-leg) ) 

(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 

(defun  animation  () 

(do  ((theta-loop  0  (+  theta-loop  1))) 

( (>  theta-loop  90)  ’end) 

(test-camera  30  theta-loop) ) ) 


-30  0  0) 


( 


i 


i 


! 
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( 


TWO  LINK  INVERTED  PENDULUM  SIMULATION  WITH  NEWTON- EULER 


;;  File  :  euler-angle-rigid-body . cl 

;  ;  Author  :  Dr .  R .  McGhee 

;  ;  :  Naval  Postgraduate  School 

;;  :  Monterey,  CA  93943 

; ;  Modified  by  :  Mehmet  Bediz 

;  ;  Summary  :  This  file  contains  rigid  body  class  which  is  implemented 

;;  :  by  Dr.  R.  McGhee .  The  function  "draw- invert ed-pendulum" 

;  ;  :  and  the  other  functions  called  by  this 

;;  :  function  are  implemented  by  Mehmet  Bediz. 

. .  ********************************************************************** 


(defclass  rigid-body 

0 

( (posture  ;The  vector  (xe  ye  ze  phi  theta  psi) . 

:initform  '(000000) 

:initarg  :posture 
: accessor  posture) 

(posture-rate  ;The  vector  (xe-dot  ye-dot  ze-dot  phi-dot  theta-dot  psi-dot) . 

: initarg  :posture-rate 
: accessor  posture-rate) 

(velocity  ;The  six-vector  (u  v  w  p  q  r)  in  body  coordinates. 

:initform  '(000  000) 

: initarg  : velocity 
: accessor  velocity) 

(velocity-growth-rate  ;The  vector  (u-dot  v-dot  w-dot  p-dot  q-dot  r-dot) . 

: accessor  velocity-growth-rate) 

( f orces-and-torques  ;The  vector  (Fx  Fy  Fz  L  M  N)  in  body  coordinates. 

: initf orm  (list  000000) 

-.accessor  forces-and- torques ) 

(moments-of-inertia  ;The  vector  (lx  Iy  Iz)  in  principal  axis  coordinates. 
:initform  '(1  100  1) 

: initarg  :moments-of-inertia 
: accessor  moments-of-inertia) 

(mass 

:initform  100 
: initarg  :mass 


: accessor  mass) 

(node-list  ; (x  y  z  1)  in  body  coord  for  each  node.  Starts  with  (0  0  0  1) . 

: initf orm  '({000  1)  (-0.5  -0.5  0.5  1)  (0.5  -0.5  0.5  1) (0.5  -0.5  -0.5  1) 

(-0.5  -0.5  -0.5  1) (-0.5  0.5  0.5  1)  (0.5  0.5  0.5  1) 

(0.5  0.5  -0.5  1 )  (-0.5  0.5  -0.5  1)  (0  0  0  1)  (0  0  3  1) ) 

: initarg  mode-list 
: accessor  node-list) 

( po lygon- list 

: initform  '( (1  5  8  4) (5  6  7  8) (6  2  3  7) (2  1  4  3) (9  10)) 

: initarg  : polygon-list 
: accessor  polygon-list) 

( transf ormed-node-list  ; (x  y  z  1)  in  earth  coord  for  each  node  in  node-list, 
rinitform  '{(0001)  (-2  -2  0  1)  (2  -2  0  1) (2  -2  -30  1) (-2  -2  -30  1) 

(-2  201)  (2  20  1) (2  2  -30  1) (-2  2  -30  1)) 

: accessor  transformed-node-list) 

(H-matrix 
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: initf orm  (unit-matrix  4 ) 

: accessor  H-matrix) 

( time- stamp 

•.accessor  time-stamp)  )  ) 

(defmethod  initialize  ( (body  rigid-body) ) 

(setf  { transf ormed-node-list  body)  (node-list  body)) 

(setf  (velocity-growth-rate  body)  (update-velocity-growth-rate  body) ) 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (time-stamp  body)  (get-internal-real-time)))) 

(defmethod  set-transf ormed-node-list-z ( (body  rigid-body)  z) 

(setf  ( third ( fourth  ( transf ormed-node-list  body)))(-  z) ) 

(setf  (third(fifth  (transf ormed-node-list  body)))  (-  z) ) 

(setf  (third(eighth  ( transf ormed-node-list  body)))  (-  z) ) 

(setf  (third(ninth  (transf ormed-node-list  body)))  (-  z) ) 

( transf ormed-node-list  body))) 

(defun  transform  (obj) 

(list  0  (first  obj) (second  obj) (third  obj))) 

(defun  reverse- transform  (obj) 

(list  (second  obj) (third  obj) (fourth  obj)  1)) 

(defmethod  move  { (body  rigid-body)  azimuth  elevation  roll  x  y  z) 

(setf  (posture  body)  (list  x  y  z  roll  elevation  azimuth) ) 

(setf  (H-matrix  body) 

(homogeneous- transform  azimuth  elevation  roll  x  y  z)) 

(transf orm-node- list  body) ) 

(defmethod  get-delta-t  ((body  rigid-body))  0.005) 

(defmethod  f orces-earth-to-body  (Fx-e  Fy-e  Fz-e  theta) 

(let*  ( ( f orces-earth  (list  Fx-e  Fy-e  Fz-e)) 

(R-matrix  (rotation-matrix  0  (-  theta)  0)) 

(forces-body  (post-multiply  R-matrix  f orces-earth) ) ) 

forces-body) ) 

(defmethod  update-rigid-body  ((body  rigid-body))  ;Euler  integration. 

(let*  ( (delta-t  (get-delta-t  body))) 

(update-posture  body  delta-t) ;  EULER 
(update-velocity  body  delta-t) ;  EULER 

(setf  (H-matrix  body)  (homogeneous -transf orm  (sixth  (posture  body)) 

(fifth  (posture  body))  (fourth  (posture  body))  (first  (posture  body)) 
(second  (posture  body))  (third  (posture  body)))) 

(transform-node-list  body) 

(update-velocity-growth-rate  body) ) ) 

(defmethod  update-velocity-growth-rate  ( (body  rigid-body) ) 

(setf  (velocity-growth-rate  body)  ;Assumes  principal  axis  coordinates  with 
(multiple-value-bind  ; origin  at  center  of  gravity  of  body. 

(Fx  Fy  Fz  L  M  N  u  v  w  p  q  r  lx  Iy  Iz)  ; Declares  local  variables, 
(values-list  ;Values  assigned. 
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{ append 

( forces -and- torques  body)  (velocity  body)  (moments-of -inertia  body))) 
(list  (+  (*  v  r)  (*  -1  w  q)  (/  Fx  (mass  body)) 

(*  * gravity*  (first  (third  (H-matrix  body) ) ) ) ) ‘ 

(+  (*  w  p)  (*  -1  u  r)  (/  Fy  (mass  body)) 

(*  *gravity*  (second  (third  (H-matrix  body))))) 

(  +  (*  u  q)  (*  -1  v  p)  (/  Fz  (mass  body)) 

(*  *gravity*  (third  (third  (H-matrix  body))))) 

(/  (+  (*  (-  Iy  Iz)  q  r)  L)  lx) 

(/  {+  (*  (-  Iz  lx)  r  p)  M)  Iy) 

(/  (  +  (*  (-  lx  Iy)  p  q)  N)  Iz)))))  ;Value  returned. 

(defmethod  update-velocity  ((body  rigid-body)  delta-t)  ;Euler  integration. 

(setf  (velocity  body) 

(vector-add  (velocity  body) 

(scalar-multiply  delta-t  (velocity-growth-rate  body) ) ) ) ) 

(defmethod  update-posture  {(body  rigid-body)  delta-t)  ;Euler  integration. 

(setf  (posture-rate  body)  (earth-velocity  body) ) 

(setf  (posture  body) 

(vector-add  (posture  body)  (scalar-multiply  delta-t  (posture-rate  body) ) ) ) ) 

(defmethod  transform-node-list  ( (body  rigid-body) ) 

(setf  ( transformed-node-list  body) 

(mapcar  #' (lambda  (node-location) 

(post-multiply  (H-matrix  body)  node-location) ) 

(node-list  body) ) ) ) 

(defconstant  ^gravity*  32.2185) 

(defmethod  earth-velocity  ( (body  rigid-body) ) 

(let*  {(linear-velocity  (firstn  3  (velocity  body))) 

(rotational-velocity  (cdddr  (velocity  body))) 

(posture  (posture  body) ) 

(R-matrix  (rotation-matrix  (sixth  posture)  (fifth  posture) 

(fourth  posture) ) ) 

(linear-earth-velocity  (post-multiply  R-matrix  linear-velocity) ) 
(T-matrix  (body-rate-to-euler-rate-matrix  (sixth  posture) 

(fifth  posture)  (fourth  posture))) 
(rotational-earth-velocity  (post-multiply  T-matrix 

rotational-velocity) ) ) 

(append  linear-earth-velocity  rotational-earth- velocity) ) ) 

(defun  test-rigid-body  () 

(setf  airplane-1  (make-instance  'rigid-body)) 

(initialize  airplane-1) 

(setf  camera-1  (make- instance  ' strobe- camera ) ) 

(move  camera-1  {-  {/  pi  2))  0  0  0  30  0  ) 

(take-picture  camera-1  airplane-1) 

(dotimes  (i  20  'done)  (update-rigid-body  airplane-1)) 

(take-picture  camera- 1  airplane-1) ) 


;  -  two  Link  Inverted  Pendulum 

;  -  Massless  leg  and  a  single  rigid  body  (fixed  leg  length) 

;  -  state  vector  x  =  (theta-1  theta-l-dot  theta-2  theta-2-dot) 

;  -  state  vector  u  =  (M-l  M-2) 

;  -  control  torque  at  the  ankle  (M-l) 

;  -  control  torque  at  the  hip  (M-2) 

;  -  Mewton-Euler  Method 

;  -  3x3  matrix  inversion  to  calculate  forces  and  moments 

;  -  Heun  Integration 


(setf  1  0.5) 

(setf  r  3) 

(setf  time-step  0.01) 


(defun  solve-three-equation-system  (state-vector  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot) 


(setf 

m  100) 

(setf 

1  0.5) 

(setf 

r  3) 

(setf 

I  100) 

(setf 

M-l  (compute' 

( setf 

M-2  (compute 

(setf 

theta-1 

(setf 

theta-l-dot 

(setf 

theta-2 

(setf 

theta-2-dot 

(setf 

g  32.2) 

(post- 

-multiply 

(matrix- inverse 
(list  (list 
(list 


M-l  state-vector  K-theta-1 
K-theta-2 
M-2  state-vector  K-theta-1 
K-theta-2 

.(first  state-vector)) 
(second  state-vector) ) 
(third  state-vector) ) 
(fourth  state-vector)) 


0  I  (*  -1  1  (sin 

( *  m  r  (cos  theta-1))  (* 
(*  -1  (sin  theta-1))) 

(list  (*  m  r  (sin  theta-1))  {* 
(cos  theta-1) ) ) ) 


K-theta-l-dot 
K-theta-2-dot) ) 
K-theta-l-dot 
K-theta-2-dot) ) 


(-  theta-2  theta-1) ) ) ) 
m  1  (cos  theta-2)) 

m  1  (sin  theta-2) ) 


(list 


(/  (*  {-  (*  M-2  (+  1  r) )  (*  1  M-l) ) (cos  (-  theta-2  theta-1))) 

(+  (*  m  r  theta-l-dot  theta-l-dot  (sin  theta-1) ) 

(*  m  1  theta-2-dot  theta-2-dot  (sin  theta-2) ) 

(/  (*  (-  M-l  M-2)  (cos  theta-1))  r) ) 

(+  (*  -1  m  r  theta-l-dot  theta-l-dot  (cos  theta-1)) 

(*  -1  m  1  theta-2-dot  theta-2-dot  (cos  theta-2)) 

(*  m  g) 

(/  (*  (-  M-l  M-2)  (sin  theta-1))  r) ) ) ) ) 


r) 


(defun  euler- step- constant- leg  (state-vector  threeXthree-matrix) 
(mapcar  '  +  state-vector  (scalar-multiply  time-step 

( derivative-state-vector 
state-vector  threeXthree-matrix) ) ) ) 
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(defun  heun-step-constant-leg  (x  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot) 

(setf  threeXthree-matrix  ( solve- three-equation- system 

x  K-theta-1  K-theta-l-dot 
K-theta-2  K-theta-2-dot ) ) 

(mapcar  '+  x  (scalar-multiply  (*  time-step  .5) 

(vector-add  (derivative-state-vector  x  threeXthree-matrix) 
(derivative-state- vector 

(euler-step-constant-leg  x  threeXthree-matrix) 
threeXthree-matrix) ) ) ) ) 

(defun  derivative-state-vector  (state-vector  threeXthree-matrix) 
(list  (second  state-vector) 

(first  threeXthree-matrix) 

(fourth  state-vector) 

(second  threeXthree-matrix) ) ) 


(defun  compute-M-1  (state-vector  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot) 

(setf  theta-1  (first  state-vector)) 

(setf  theta-l-dot  (second  state-vector) ) 

(  +  (*  -1  K-theta-1  theta-1) (*  -1  K-theta-l-dot  theta-l-dot))) 

(defun  compute-M-2  (state-vector  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot ) 

(setf  theta-2  (third  state-vector) ) 

(setf  theta-2-dot  (fourth  state-vector)) 

(+  (*  -1  K-theta-2  theta-2) (*  -1  K- theta-2-dot  theta-2-dot) ) ) 

(defun  compute-y  (state-vector) 


(setf 

theta-1 

(first 

state-vector) ) 

(setf 

theta-l-dot 

( second 

state-vector) ) 

(setf 

theta-2 

(third 

state-vector) ) 

(setf 

theta-2-dot 

( fourth 

state-vector) ) 

(setf 

1  0.5) 

(setf 

r  3) 

(+  (* 

r  (sin  theta- 

1)  ) 

(* 

1  (sin  theta- 

2)  )  )  ) 

(defun  compute-z  (state-vector) 

( setf 

theta-1 

(first 

state-vector) ) 

(setf 

theta-l-dot 

(second 

state-vector) ) 

(setf 

theta-2 

(third 

state-vector) ) 

(setf 

theta-2-dot 

(fourth 

state-vector) ) 

(setf 

1  0.5) 

(setf 

r  3) 

(+  (*  r  (cos  theta-1)) 

(*  1  (cos  theta-2) ) ) ) 


(defun  draw- inverted-pendulum  (halt-time  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot ) 
(setf  robot-body  (make- instance  ’rigid-body)) 
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(initialize  robot-body) 


(setf  robot-leg  (make-instance  'rigid-body)) 

(initialize  robot-leg) 

(setf  camera-1  (make-instance  ’strobe-camera)) 

(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -10  0  0) 

(setf  graph- 1 -window  ( make -window- graph-1 ) ) 

(draw-coordinate-axes  graph- 1-window) 

(setf  graph-2 -window  (make-window-graph-2)) 

(draw-coordinate-axes  graph-2 -window) 

(do  ( (x  '(0.5  0  1.0  0)  (heun-step-constant-leg  x  K-theta-1  K-theta-l-dot 

K-theta-2  K-theta-2-dot) ) 

( old-x  ' (0.5  0  1.0  0)  x) 

(time  0  (+  time  1) ) ) 

( (>  time  halt-time)  ’done) 

(move  robot-body  (deg-to-rad  0) (deg-to-rad  0)  (third  x) 

0  (compute-y  x)  {*  -l(compute-z  x) ) ) 

(setf  ( transf ormed-node-list  robot-leg)  (list  (list  0001) 

(list  0  (*  r  (sin  (first  x) ) ) 

(*  -1  r  (cos  (first  x) ) )  1) 
(list  0001))) 

(setf  (polygon-list  robot-leg)  (list  (list  2  1))) 

(cw: clear  (camera-window  camera-1)) 

(take-picture  camera-1  robot-body) 

(take-picture  camera-1  robot-leg) 

(draw-line-in-window  graph- 1 -window  80  (list  (*  time-step  (-  time  1)) 

(first  old-x)) 

(list  (*  time-step  time) 

(first  x) ) ) 

(dr aw- line- in-window  graph- 2 -window  80  (list  (*  time-step  (-  time  1)) 

(third  old-x)) 

(list  ( *  time-step  time) 

(third  x) ) ) ) ) 

(defun  make-window-graph- 1  () 

(cw: make-window-stream  : borders  5 

: left  0 
: bottom  50 
: width  450 
: height  450 

: title  "Leg  Angle  (theta-1)  NEWT ON- EULER " 

: activate-p  t) )  ;Make  window  visible. 

(defun  make-window-graph-2  () 

(cw: make-window- stream  : borders  5 

: left  0 
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bottom  550 
width  450 
height  450 

title  "Body  Attitude  (theta-2)  NEWTON-EULER 


activate-p  t) )  ;Make  window  visible. 


Ill 


********************************************************************* 
File  :  load-euler-f iles . cl 

Author  :  Mehmet  Bediz 

Naval  Postgraduate  School 
Monterey,  CA  93943 
Summary  :  This  file  contains  functions  to  load  files. 

********************************************************************* 


;  Load  files 


(load  " harmonic -equation. cl " ) 

(load  M robot-kinematics . cl " ) 

( load  " euler-angle-rigid-body . cl M ) 
(load  " strobe-camera . cl " ) 
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.  .  *********************************************************************** 
r  t 

;;  File  :  strobe-camera . cl 

;;  Author  :  Dr.  R.  McGhee 

; ;  :  Naval  Postgraduate  School 

; ;  :  Monterey,  CA  93943 

;;  Summary  :  This  file  contains  strobe-camera  class  definitions. 

.  .  ************************************************************************ 

(require  :xcw) 

(cw: initialize-common-windows ) 

(defclass  strobe-camera  (rigid-body) 

( (focal-length 

: accessor  focal-length 
:initform  6) 

( camera-window 
: accessor  camera-window 

linitform  (cw: make-window-stream  :borders  5 

: left  470 
: bottom  500 
: width  650 
: height  650 

: title  "Two  Link  Inverted  Pendulum 

(Constant  Length  Massless  Leg) 
NEWTON-EULER" 

:activate-p  t) ) 

(H-matrix 

rinitform  (homogeneous-transform  .3  -.3  0  -300  -90  -90)) 

( inverse-H-matrix 
: accessor  inverse-H-matrix 

:initform  (inverse-H  (homogeneous-transform  .3  -.3  0  -300  -90  -90))) 
(enlargement- factor 
: accessor  enlargement-factor 
: initf orm  30 ) ) ) 

(defmethod  move  ( (camera  strobe-camera)  azimuth  elevation  roll  x  y  z) 

(setf  (H-matrix  camera)  (homogeneous-transform  azimuth  elevation  roll  x  y  z) ) 
(setf  (inverse-H-matrix  camera)  (inverse-H  (H-matrix  camera) ) ) ) 

(defmethod  take-picture  ( (camera  strobe-camera)  (body  rigid-body) ) 

(let  ((camera-space-node-list  (mapcar  #' (lambda  (node-location) 

(post-multiply  (inverse-H-matrix  camera)  node-location) ) 
(transformed-node-list  body)  )  )  ) 

(dolist  (polygon  (polygon-list  body) ) 

(clip-and-draw-polygon  camera  polygon  camera-space-node-list ) ) ) ) 

(defmethod  clip-and-draw-polygon 

( (camera  strobe-camera)  polygon  node-coord-list) 

(do*  ({initial-point  (nth  (first  polygon)  node-coord-list)) 

(from-point  initial-point  to-point) 

(remaining-nodes  (rest  polygon)  (rest  remaining-nodes ) ) 

(to-point  (nth  (first  remaining-nodes)  node-coord-list) 

(if  (not  (null  (first  remaining-nodes))) 

(nth  (first  remaining-nodes)  node-coord-list)))} 

( (null  to-point) 

(draw-clipped-projection  camera  from-point  initial-point) ) 
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(draw-clipped-projection  camera  from-point  to-point) ) ) 


(defmethod  draw-clipped-projection  ( (camera  strobe-camera)  from-point  to-point) 
(cond  ((and  (<=  (first  from-point)  (focal-length  camera)) 

(<=  (first  to-point)  (focal-length  camera)))  nil) 

( (<=  (first  from-point)  (focal-length  camera)) 

(draw- line- in-camera-window  camera 

(perspective- transform  camera  (from-clip  camera  from-point  to-point)) 
(perspective-transform  camera  to-point) ) ) 

( (<=  (first  to-point)  (focal-length  camera)) 

(draw- line- in-camera-window  camera 

(perspective- transform  camera  from-point) 

(perspective- transform  camera  (to-clip  camera  from-point  to-point) ) ) ) 
(t  (draw- line -in- camera- window  camera 

(perspective- transform  camera  from-point) 

(perspective- transform  camera  to-point) ) ) ) ) 

(defmethod  from-clip  ( (camera  strobe-camera)  from-point  to-point) 

(let  ((scale-factor  (/  (-  (focal-length  camera)  (first  from-point)) 

(-  (first  to-point)  (first  from-point))))) 

(list  (+  (first  from-point) 

(*  scale-factor  (-  (first  to-point)  (first  from-point)))) 

(+  (second  from-point) 

{*  scale-factor  (-  (second  to-point)  (second  from-point)))) 

(+  (third  from-point) 

( *  scale-factor  (-  (third  to-point)  (third  from-point))))  1))) 

(defmethod  to-clip  ( (camera  strobe -earner a)  from-point  to-point) 

(from-clip  camera  to-point  from-point)) 

(defmethod  draw- line- in-camera-window  ( (camera  strobe-camera)  start  end) 

(cw: draw- line  (camera- window  camera) 

(cw:make-position  :x  (+  150  (first  start)) 

: y  (+  150  (second  start) ) ) 

(cw: make-position  :x  (+  150  (first  end)) 

:y  (+  150  (second  end) ) ) 

: brush-width  0)) 

(defmethod  perspective-transform  ( (camera  strobe- camera)  point-in-camera-space) 
(let*  ( (enlargement- factor  ( enlargement- f actor  camera)) 

(focal-length  (focal-length  camera)) 

(x  (first  point-in-camera-spa ce) )  ;x  axis  is  along  optical  axis 
(y  (second  point-in-camera-space))  ;y  is  out  right  side  of  camera 
(z  (third  point-in-camera-space)))  ;z  is  out  bottom  of  camera 
(list  (  +  (round  (*  enlargement-factor  (/  (*  focal-length  y)  x) ) ) 

150)  ; to  right  in  camera  window 

(+  150  (round  (*  enlargement-factor  (/  (*  focal-length  (-  z) )  x) ) 

) ) ) ) )  ; up  in  camera  window 

(defun  test-camera  (z  theta)  ; Produces  top  view  of  default  rigid-body. 

(setf  robot-leg  (make-instance  'rigid-body)) 

(initialize  robot-leg) 

(set-transf ormed-node-list-z  robot-leg  z) 

(set-transf ormed-node-list-theta  robot-leg  theta) 

(setf  camera-1  (make-instance  'strobe-camera)) 


114 


(move  camera-1  (deg-to-rad  0) (deg-to-rad  0) (deg-to-rad  0)  -30  0  0) 
(take-picture  camera-1  robot-leg)) 

(defun  deg-to-rad  (angle)  (*  .01745329251994329  angle)) 

(defun  animation  () 

(do  ((theta-loop  0  (+  theta-loop  1))) 

( (>  theta-loop  90)  'end) 

(test-camera  30  theta-loop))) 
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APPENDIX  B:  KINEMATIC  SIMULATION  SOFTWARE 


//  File  :  main.C 

//  Author  :  Mehmet  Bediz 

//  :  Naval  Postgraduate  School 

//  :  Monterey,  CA  93943 

//  Created  :  November  1996 

//  Summary  :  This  file  contains  the  main  function  and  three  motion 

//  :  functions  of  the  kinematic  model  Dynaman:  forward  stepping, 

//  :  upward  stepping,  and  jumping. 


#include  <string.h> 

#include  "main.h” 

void  step_forward(int  number_of_steps,  float  X,  float  Y,  float  Z); 
void  step-up ward(int  number_of_steps,  float  X,  float  Y,  float  Z); 
void  jump(float  X, float  Y,float  Z); 

//  Speed  of  the  animation  can  be  controlled  by  changing  delta_t 
float  delta J:  =  2  *  3.0; 


pfNode 

*root; 

pfDCS 

*dcs; 

pfMatrix 

mat,  orbit; 

pf Sphere 

sphere; 

pfNode 

*  model  1,  *model2,  *model3,  *model4; 

pfNode 

*model8,  *modell0; 

pfNode 

*modelll,  *modell2,  *modell3,  *modell4; 

pfDCS 

*nodell,  *nodel2,  *nodel3,  *nodel4; 

pfDCS 

*nodel,  *node2,  *node3,  *node4; 

pfDCS 

*node8,  *nodel0; 

pfDCS 

*dcs0; 

pfDCS 

*dcsl,  *dcs2,  *dcs3; 

pfDCS 

*dcs4,  *dcs5,  *dcs6; 

pfDCS 

*dcs7,  *dcs8,  *dcs9,  *dcsl0; 

pfDCS 

*dcsll,  *dcsl2,  *dcsl3; 

pfDCS 

*dcsl4,  *dcsl5,  *dcsl6; 

pfDCS 

*dcs30,  *dcs31,  *dcs32,  *dcs33; 

pfDCS 

*dcs34,  *dcs35,  *dcs36; 

pfDCS 

*dcs37,  *dcs38,  *dcs39; 

char 

*filel,  *file2,  *file3,  *file4; 

char 

*file8,  *filel0; 

char 

*filell,  *  file  12,  *filel3,  *filel4; 

pfLightSource  *light; 

pfMatrix 

lightMat; 

float 

d; 

pfTexture 

*tex; 

pfFrustum 

*Frust; 

void 

*  arena; 

pfDCS  ' 

*lightDCS; 
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int 

float 


i; 

fmal_position; 


r  Function: 

main 

'  Returns: 

None 

'  Parameters: 

None 

r  Summary: 

! 

Initializes  IRIS  Performer™,  loads  body  parts  from  poly  format 
files  to  DCS  nodes,  creates  the  channels  at  the  upper  right 
comer  of  the  window,  calls  step_forward,  step_upward,  and 
jump  functions  with  the  number  of  steps  and  the  initial 
positions. 

void 

main  (int  argc,  char  *argv[]) 

{ 


/*  choose  default  objects  of  none  specified  */ 

filel  =  "lowerleg.poly"; 

file2  =  "upperleg.poly,'; 

file3  =  ’’head.iv"; 

file4  =  "torso.poly"; 

file8  =  "foot.poly"; 

filelO  =  "floor.iv"; 

filel  1  =  "rightupperarm.poly"; 

filel2=  Mlowerarm.poly"; 

filel  3  =  "hand.poly"; 

filel  4  =  "leftupperarm.poly"; 

if  (  !  strcmp(argv[l],"slow”)){ 
delta_t  =  2  *  0.5; 

} 

#ifndef  IRISGL 

printfC "Sorry,  shadows  doesn't  work  in  OPENGLAn”); 
return  0; 

#endif 

/*  Initialize  Performer  */ 
pflnit(); 

/*  Use  default  multiprocessing  mode  based  on  number  of 

*  processors. 

*/ 

pfMultiprocess(PFMP_DEFAULT); 

/**  allocate  shared  memory  **/ 

InitSharedQ; 

/*  Configure  multiprocessing  mode  and  start  parallel 

*  processes. 

*/ 

pfConfigO; 
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/**  Initialize  Performer  utility  and  GUI  functions  **/ 
pfuInitUtil(); 

pflnitClock(O.Of); 

Shared->simTime  =  pfGetTime(); 
srand(Shared->simTime*  1 0000000); 

/*  Append  to  PFPATH  additional  standard  directories  where 

*  geometry  and  textures  exist 
*1 

pfFilePath(n.:/usr/share/Performer/data:/tmp_mnt/workb/bediz/Performer/data"); 

/*  Do  not  use  FLAT_  primitives  because  they  look  bad 

*  with  local  lighting. 

*/ 

pfdBldrMode(PFDBLDR_MESH_LOCAL_LIGHTING,  1 ); 

/*  Read  a  single  file,  of  any  known  type.  */ 
if  ((root  =  pfdLoadFileC'text.iv"))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 


/*  Load  the  files  */ 

if  ((model  1  =  pfdLoadFile(filel))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((model2  =  pfdLoadFile(file2))  =  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((modeB  =  PfdLoadFile(file3))  ==  NULL) 

{ 

plExit(); 

exit(-l); 

} 

if  ((model4  =  pfdLoadFile(file4))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((model8  =  PfdLoadFiIe(file8))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((model  10  =  pfdLoadFile(filelO))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 
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if  ((model  1 1  =  pfdLoadFile(filel  1))  ==  NULL) 


{ 

pfExit(); 
exit(- 1 ); 

} 

if  ((model  12  =  pfdLoadFile(filel2))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((modell3  =  pfdLoadFile(filel3))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 

if  ((model  14  =  pfdLoadFile(filel4))  ==  NULL) 

{ 

pfExit(); 

exit(-l); 

} 


/*  Create  and  attach  loaded  subgraph  to  a  pfScene.  */ 
Shared->scene  =  new  pfScene; 
dcs  =  new  pfDCS; 
dcs->addChild(root) ; 

Shared->scene->addChild(dcs); 

/*  determine  extent  of  scene’s  geometry  */ 
Shared->scene->getB  ound(&(Shared->bsphere)) ; 


yf***^**^*^*^********  main  WINDOW  ****************/ 

/*  Configure  and  open  GL  window  */ 

Shared->p  =  pfGetPipe(O); 

Shared->p2  =  pfGetPipe(O); 

Shared->pw  =  new  pfPipeWindow(Shared->p); 
Shared->pw->setName("DYNAMAN”); 
Shared->pw->setConfigFunc(OpenPipeWin); 
Shared->pw->setOriginSize(120,  120,  1100,  1100); 
Shared->pw->config(); 

/*  initializes  mouse  and  keyboard  inputs  to  be  read  from  window  */ 
pfulni tInput(S  hared->p w ,  PFUINPUT_GL) ; 
pfuInitGUI(Shared->pw); 
pfuEnableGUI  (TRUE) ; 

/*  Create  and  configure  a  pfChannel.  *1 
Shared->chan  =  new  pfChannel(Shared->p); 
Shared->chan->setScene(Shared->scene); 
Shared->chan->setNearFar(1.0f,  5. Of  *  Shared->bsphere. radius); 
Shared->chan->setFOV(45.0f,  O.Of); 

Shared->view.xyz.set(1.3f  *  Shared->bsphere.radius, 

-2.5f  *  Shared->bsphere. radius, 

3. Of  *  Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f,  -45. Of,  0.0f); 

Shared->chan->setView(Shared->view.xyz,  Shared->view.hpr); 
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Shared->chan->setTravFunc(PFTRAV_DRAW,DrawChannel); 

/********  FIRST  CHANNEL  **************************/ 
Shared->chan2[0]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[0]); 
Shared->chan2[0]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[0]->setScene(Shared->scene); 
Shared->chan2[0]->setNearFar(1.0f,  5.0f  *  Shared->bsphere. radius); 
Shared->chan2[0]->setFO V (45  .Of,  O.Of) ; 

Shared->view.xyz.set(  Shared->bsphere.radius, 

-4.0f  *  Shared->bsphere.radius, 

0.8f  *  Shared->bsphere.radius); 
Shared->view.hpr.set(O.Of,  O.Of,  O.Of); 

Shared->chan2[0]->setView(Shared->view.xyz,  Shared->view.hpr); 

I*.  *  *  *  *  *  *  *  SECOND  CHANNEL  **************************/ 

Shared->chan2[l]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[  1  ]); 

Shared->chan2[  1  ]->setTravFunc(PFTR  AV  _DRA  W,DrawChannel); 
Shared->chan2[  1  ]->setScene(Shared->scene); 
Shared->chan2[l]->setNearFar(1.0f,  5. Of  *  Shared->bsphere. radius); 
Shared->chan2[  1  ]->setFO V (45 .Of,  O.Of); 

Shared->view.xyz.set(1.3  *  Shared->bsphere.radius, 

-2.5f  *  Shared->bsphere.radius, 

3.0f  *  Shared->bsphere.radius); 
Shared->view.hpr.set(O.Of,  -45.0f,  O.Of); 

Shared->chan2[l]->setView(Shared->view.xyz,  Shared->view.hpr); 

/********  THIRD  CHANNEL  **************************/ 

Shared->chan2[2]  =  new  pfChannel(Shared->p); 

S  hared->p  w->addChan(  S  hared->chan2  [  2] ) ; 

Shared->chan2[2]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[2]->setScene(Shared->scene); 
Shared->chan2[2]->setNearFar(1.0f,  5.0f  *  Shared~>bsphere.  radius); 
Shared->chan2[2]~>setFO  V (45 .Of,  O.Of) ; 

Shared->view.xyz.set(  1.6  *  Shared->bsphere.radius, 

-0.15f  *  Shared->bsphere. radius, 

2.5f  *  Shared->bsphere.radius); 
Shared->view.hpr.set(0.0f,  -90.0f,  O.Of); 

Shared->chan2[2]->setView(Shared->view.xyz,Shared->  view.hpr); 

/********  POURTH  CHANNEL  **************************/ 

Shared->chan2[3]  =  new  pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[3]); 
Shared->chan2[3]->setTravFunc(PFTRAVJDRAW,DrawChannel); 
Shared->chan2[3]->setScene(Shared->scene); 
Shared->chan2[3]->setNearFar(1.0f,  5.0f  *  Shared->bsphere.radius); 
Shared->chan2[3]->setFO V (45  .Of,  O.Of) ; 

Shared->view.xyz.set(  4. Of  *  Shared->bsphere.radius, 

-0.5f  *  Shared->bsphere.radius, 

0.5  *  Shared->bsphere.radius); 
Shared->view.hpr.set(90.0f,  O.Of,  O.Of); 
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Shared->chan2[3]->setView(Shared->view.xyz,  Shared->view.hpr); 
Shared->chan2[3]->setViewport(0.7,  0.85,  0.7,0.85); 
Shared->chan2[2]->setViewport(0.85,  1.0,  0.7,  0.85); 
Shared->chan2[l]->setViewport(0.85,  1.0,  0.85,  1.0); 
Shared->chan2[0]->setViewport(0.7,  0.85,  0.85,  1.0); 

orbit. makeRot(l. Of,  O.Of,  O.Of,  1  .Of); 

/*  scale  models  to  unit  size  */ 
nodel  =  new  pfDCS; 
node  1  ->addChild(model  1 ); 
model  1  ->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node  1  ->setScale(  1  .Of/sphere.radius) ; 

node2  =  new  pfDCS; 
node2->addChild(model2); 
modeI2->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node2->setScale(l. Of/sphere.radius); 

node3  =  new  pfDCS; 
node3->addChild(model3 ) ; 
model2->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node3->setScale(l.  Of/sphere.radius); 

node4  =  new  pfDCS; 
node4->addChild(model4) ; 
model4->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node4->setScale(  1  .Of/ sphere.radius) ; 

node 8  =  new  pfDCS; 
node8->addChild(modeI8); 
model8->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

node8->setScale(  1  .Of/sphere.radius); 

node  10  =  new  pfDCS; 
node  1 0->addChild(model  1 0); 
model  1 0->getBound(&sphere) ; 
if  (sphere.radius  >  O.Of) 

nodel0->setScale(l. Of/sphere.radius); 

nodel  1  =  new  pfDCS; 
nodel  l->addChild(modell  1); 
model  1  l->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

nodelO->setScale(l. Of/sphere.radius); 

node  12  -  new  pfDCS; 
node  1 2->addChild(model  1 2); 
model  12->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 
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node  10~>setScale(  l.Of/ sphere.  radius); 

node  13  =  new  pfDCS; 
node  1 3->addChild(model  13); 
model  13->getBound(&sphere); 
if  (sphere.radius  >  O.Of) 

nodel0->setScale(1.0f/sphere.radius); 

node  14  =  new  pfDCS; 
node  1 4->addChild(model  1 4) ; 
model  1 4->getB  ound(&sphere); 
if  (sphere.radius  >  O.Of) 

nodel  0->setScale(  l.Of/sphere.radius); 

/*  Lighting  without  shadowing  */ 

//  put  a  default  light  source  in  the  scene 
Shared->scene->addChild(new  pfLightSource) ; 

arena  =  pfGetS  hared  Arena(); 

//  Create  and  configure  shadow  frustum.  Fit  frustum  tightly  to  scene. 
Frust  =  new(arena)  pfFrustum; 

Frust->makeSimple(FOV); 

d  =  Shared->bsphere.radius  /  sinf(PF_DEG2RAD(FOV/2.0f)); 
d  =  PF_MAX2(d,  NEAR  +  S hared ->bsphere. radius); 

Frust->setNearFar(NEAR,  d  +  l.lf  *  Shared->bsphere.radius); 
tex  =  initSpotTex(); 

light  =  new  pfLightSource; 
light->setMode(PFLSJPROJTEX_ENABLE,  1 ) ; 
light->setAttr(PFLS_PROJ_FRUST,  Frust); 
light->set Attr(PFLS_PROJ_TEX,  tex) ; 
light->setColor(PFLTJDIFFUSE,  l.Of,  l.Of,  l.Of); 
light->setVal(PFLS_INTENSITY,  3. Of); 
light->setPos(17.0  ,-7.0, 13.9,  l.Of);// Make  light  local 
light->setSpotDir(O.Of,  O.Of,  -lO.Of); 

//  Make  DCS  to  move  lights  around 
lightDCS  =  new  pfDCS; 
lightDCS->addChild(light); 

l*  Main  DCS  */ 
dcsO  =  new  pfDCS; 

/*  Left  Foot  */ 
dcsl  =  new  pfDCS; 
dcsl  ->  addChild(nodel); 

/*  Left  lowerleg  */ 

/*  Right  lowerleg  */ 
dcs4  =  new  pfDCS; 
dcs4  ->  addChild(nodel); 
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/*  Left  upperleg  */ 
dcs2  =  new  pfDCS; 
dcs2  ->  addChild(node2); 

/*  Right  upperleg  */ 
dcs5  =  new  pfDCS; 
dcs5  ->  addChild(node2); 

/*  Floor  */ 
dcslO  =  new  pfDCS; 
dcslO  ->  addChild(nodelO); 
dcs30  =  new  pfDCS; 
dcs30  ->  addChild(nodelO); 
dcs31  =  new  pfDCS; 
dcs31  ->  addChild(nodelO); 
dcs32  =  new  pfDCS; 
dcs32  ->  addChild(nodelO); 
dcs33  =  new  pfDCS; 
dcs33  ->  addChild(nodelO); 
dcs34  =  new  pfDCS; 
dcs34  ->  addChild(nodelO); 
dcs35  =  new  pfDCS; 
dcs35  ->  addChild(nodelO); 
dcs36  =  new  pfDCS; 
dcs36  ->  addChild(nodelO); 
dcs37  =  new  pfDCS; 
dcs37  ->  addChild(nodelO); 
dcs38  =  new  pfDCS; 
dcs38  ->  addChild(nodelO); 
dcs39  =  new  pfDCS; 
dcs39  ->  addChild(nodelO); 

/*  Torso  (Two  parts)  */ 
dcs6  =  new  pfDCS; 
dcs6  ->  addChild(node4); 
dcsO  ->  addChild(dcs6); 

dcs3  =  new  pfDCS; 
dcs3  ->  addChild(node4); 
dcsO  ->  addChild(dcs3); 

/*  Left  Legs  */ 
dcs2  ->  addChild(dcsl); 
dcsO  ->  addChild(dcs2); 

/*  Right  Legs  */ 
dcs5  ->  addChild(dcs4); 
dcsO  ->  addChild(dcs5); 

/*  Feet*/ 
dcs8  =  new  pfDCS; 
dcs8  ->  addChild(nodeS); 
dcs9  =  new  pfDCS; 
dcs9  ->  addChild(nodeS); 


dcsl  ->  addChild(dcs8); 
dcs4  ->  addChild(dcs9); 

/*  Head  */ 
dcs7  =  new  pfDCS; 
dcs7  ->  addChild(node3); 
dcs3  ->  addChild(dcs7); 

/*  Right  upper  arm  */ 
dcsll  =  new  pfDCS; 
dcsll  ->  addChild(nodell); 
dcsO  ->  addChild(dcsl  1); 

/*  Right  lower  arm  */ 
dcsl 2  =  new  pfDCS; 
dcsl 2  ->  addChild(nodel2); 
dcsll  -> addChild(dcsl2); 

/*  Right  hand  */ 
dcsl 3  =  new  pfDCS; 
dcsl3  ->  addChild(nodel3); 
dcsl 2  ->  addChild(dcs!3); 

/*  Left  upper  arm  */ 
dcsl 4  =  new  pfDCS; 
dcsl 4  ->  addChild(nodel4); 
dcsO  ->  addChild(dcsl4); 

/*  Left  lower  arm  */ 
dcsl 5  =  new  pfDCS; 
dcsl5  ->  addChild(node!2); 
dcsl 4  ->  addChild(dcsl5); 

/*  Left  hand  */ 
dcsl 6  =  new  pfDCS; 
dcsl 6  ->  addChild(nodel3); 
dcsl 5  ->  addChild(dcsl6); 

//  Stairs 

dcs30  ->  addChild(dcs31); 
dcs31  ->  addChild(dcs32); 
dcs32  ->  addChild(dcs33); 
dcs33  ->  addChild(dcs34); 
dcs34  ->  addChild(dcs35); 
dcs35  ->  addChild(dcs36); 
dcs36  ->  addChild(dcs37); 
dcs37  ->  addChild(dcs38); 
dcs38  ->  addChild(dcs39); 

Shared->scene->  addChild(dcsO); 
Shared->scene->  addChild(dcs30); 

I*  Create  "floor  letters"  */ 
dcs  ->  addChild(initFloor(&Shared->bsphere)); 


/*  Floor  *1 
dcsl0->setScale(2.0f); 
dcsl0->setRot(0.0,  -90.0  ,0.0); 
dcsl0->setTrans(-1.0  ,  -11.0 , 0.05); 

//  Draw  steps 
dcs30->setScale(0.5f); 
dcs30->setRot(-90.0,  -90.0  ,0.0); 
dcs30->setTrans(13.0  ,  0.0 , 0.05); 
dcs31->setTrans(0.0  ,  -2.0 , 5.4); 
dcs32->setTrans(0.0  ,-2.0 ,4.1); 
dcs33->setTrans(0.0  ,-1.9, 4.1); 
dcs34->setTrans(0.0  ,-1.9, 4.1); 
dcs35->setTrans(0.0  ,-1.9, 4.1); 
dcs36->setTrans(0.0  ,-1.8, 4.1); 
dcs37->setTrans(0.0  ,  -1.8 , 4.1); 
dcs38->setTrans(0.0  ,  -1.8 , 4.1); 
dcs39->setTrans(0.0  ,  -1.7 , 4. 1); 

/*  Set  up  initial/default  view  */ 

MakcGUIO; 

int  temp  =  system("sfplay  runaway.wav  &"); 
if  (-1  =  temp){ 
printff 'Can't  play  \n"); 

} 

else{ 

printf(nPlaying  %i  \n",temp); 

} 

for  (int  forever  =  0;  forever  <  50;  forever  ++){ 
step_forward(6,  -9.8  ,  -7.0 , 3.9); 
step_upward(5,  13.3  ,  -7.0 , 3.9); 
jump(3 1.7,  -7.0, 12.496556); 
step_forward(4,  39.137990  ,  -7.0 , 4.670563); 

} 

//  Kill  music 
system(',music_killerM); 

/*  Terminate  parallel  processes  and  exit.  */ 
p£Exit(); 


// - 

//  Function: 

//  Returns: 

//  Parameters: 
//  Summary: 

// 

// 

// 

// 

// - 


step_forward 

None 

Number  of  steps  needs  to  be  taken,  initial  position 
Computes  the  joint  angles  according  to  the  position 
of  the  end  effector(foot)  by  using  the  inverse  kinematic 
equations  of  the  three  link  planar  manipulator.  The 
algorithm  for  the  path  of  the  foot  is  described  in 
Chapter  IV  of  this  thesis  as  “Stepping  Forward  Algorithm”. 


void  step_forward(int  number_of_steps, float  X, float  Y, float  Z){ 

float  11  =  l.Of; 
float  12  =  1.  Of; 
float  c  1  _left,c  1  right; 
float  theta l,theta2; 
float  kl  Jeft,k2_left; 
float  kl_right,k2_right; 

float  x_left  =  1.732f; 
float  y_left  =  O.Of; 
float  x_right  =  1.732f; 
float  y_right  =  O.Of; 

//  INITIAL  HALF  STEP 
for  (float  z  =  32  ;z<43;z+=0.5*  delta_t) 

{ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 

Shared->simTime  -  pfGetTime(); 

/*  Main  Body  *1 
dcs0->setRot(90.0,  90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 ,  180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 
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/*  Left  iowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 


/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  Iowerleg  */ 
dcsl->setTrans(0  ,  -1.7,  0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7,  0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcs!4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5 ,  -4.6,  0.0); 

I*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

iiiiiiiiiiiniiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiin 

II  Inverse  Kinematics 

II 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIH 

ll  SECOND  STEP 
If  RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  z)  *  DEG_TO_RAD  )* 

(11  + 12)  *  cos((64  -  2*  z)  *  DEG_TO_RAD); 
y_right  =  0.95f  *  cos((64  -  2*  z)  *  DEG  _TO_RAD  )* 

(11  + 12)  *  sin((64  -  2*  z)  *  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right) 

-  (11*11) -(12*12))/(2*  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl  .right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

theta  1  =  atan(y_right/x„right)  -  atan(k2_right/kl bright); 
//  Right  Leg 

dcs2->setRot(0  ,(57.3  *  thetal),0  ); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 
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//  Left  Arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  z)  *  DEG_TO_RAD  )  *  (11  + 12)  * 
cos((-64  +  2*  z)  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  z)  *  DEGJTO_RAD  )  *  (11  + 12)  * 
sin((-64  +  2*  z)  *  DEG_TO_RAD); 
cl_left=  ((x_left*x_left)  +  (y_left*y_left) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl  _left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal  =  atan(y_left/x_left)  -  atan(k2Jeft/kl  Jeft); 

//Left  leg 

dcs5->setRot(0  ,(57.3  *  thetal),  0); 
dcs4->setRot(0  ,(57.3  *  theta2),  0); 

//  Right  arm 

dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0); 
dcsl2~>setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.0f),0); 

//  Torso  rotation 
if(z  <  31){ 

dcs3->setRot((-(z  -  21)/3.0),0  ,0); 
dcs6->setRot((-(z  -21)/3.0),0  ,0); 

} 

else{ 

dcs3->setRot(((-20  +  ((z-  21)))/3.0),0  ,0); 
dcs6->setRot(((-20  +  ((z-  21)))/3.0),0  ,0); 

} 


dcsO->setTrans(X  +  (z  ~  32)  *  0.1  ,Y, 

(Z  +  0.2)  +  0.08f  *  cos(2.0f  *  3.14159f  *  (z  -  41)  /  21. Of)); 

UpdateView(); 

UpdateGUIO; 

pfFrame(); 

}//End  of  second  for 

for(  i  =  1  ;  i  <  number_of_steps  ;  i++){ 

for  ( z  =  (i-l)*42  +  1  ;  z  <  (i-l)*42  +  22  ;  z  +=  0.5  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 

Shared->simTime=pfGetTime(); 


/*  Main  Body  */ 
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dcs0->setRot(90.0,  90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,  -1.7,  0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5~>setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsl  l->setTrans(-1.0 , 1.7,  0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcs!3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcs  1 4->setScale(0.25f); 
dcsl4->setTrans(1.6  , 1.8,  0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 


illlllllllllllllllllllllllllllllllllllllllllllllllllllll 
H  Inverse  Kinematics 

lllllllllllllllllllllllllllIIIIIIIIIIIIIIIIIIIHIIHIIll 

//  FIRST  STEP 
//  LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG  JTOJRAD  ) 
*(11  + 12)  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG  JTOJRAD); 
yjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 
*(11  +  12)  *  sin((22  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 

cl  Jeft=  ((xjeft*xjeft)  +  (yjeft*yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl  Jeft)); 
kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2_Ieft  =  12  *  sin(theta2); 
theta  1  =  atan(y_left/x_left)  -  atan(k2_left/kl_left); 

dcs5->setRot(0  ,(57.3  *  thetal),0 ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 

//  Right  Arm 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  Right  Leg 

xjright  =  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  + 12)  *  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG  JTOJRAD); 
y_right  =  cos((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEGJTOJRAD  ) 

*  (11  +  12)  *  sin((-22  +  2  *  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (y_right*y_right) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_right)); 

kl_right  =  11+  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  Arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  Torso  rotation 
if((z-  42  *(i  -  1))  <  1 1){ 
dcs3->setRot(((z-  42  *(i  -  l))/3.0),0  ,0); 
dcs6->setRot(((z-  42  *(i  -  l))/3.0),0  ,0); 

} 

else{ 

dcs3->setRot(((20  -  (z-  42  *(i  -  l)))/3.0),0  ,0); 
dcs6->setRot(((20  -  (z-  42  *(i  -  1)))/3.0),0  ,0); 

} 

dcs0->setTrans(  X  +  1.0  +  z  *  0.1  ,  Y  ,  Z  +  0.2  +  0.08f 
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*cosj;2.0f  *  3.14159f  *  z  /  21. Of)); 


UpdateViewO; 

UpdatcGUIO; 

pfFrame(); 

} 

for  (  z  =  (i-l)*42  +  22  ;  z  <  (i-l)*42  +  43  ;  z  +=  0.5  *  delta„t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 

Shared->simTime  =  pfGetTime(); 

/*  Main  Body  */ 
dcs0->setRot(90.0,  90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,  -1.7, 0); 

/*  Right  foot  */ 
dcs8->setS  cale(0. 7f) ; 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

I*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7, 0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 
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/*  Left  upper  arm  */ 
dcs  1 4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

llllllllllllllllillllllllllllllllllllllllllllllllllHIli 
//  Inverse  Kinematics 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIllllllllilllllllllllllllllH 


lilllllllllllllllllllllllillllllllllllllllimiUUHHi 

fl  SECOND  STEP 
//  RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
y.right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  sin((64  -  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right) 

-  (11*11)  -  (12*12) )/(2  *  11  *12); 
theta2  =  (1  *acos(cl  fright)); 

kl_right  =  II  +  (12  *  cos(theta2)); 
k2„right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  Leg 

dcs2->setRot(0  ,(57.3  *  thetal ),0  ); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  ARM 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.0f),0); 

//  LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD  ) 

*  (11  +  12)  *  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG_TOJRAD  ) 

*  (11  + 12)  *  sin((-64  +  2*  (z-  42  *(i  -  1)))  *  DEG„TO_RAD); 

cl_left=  ((x_left*x_left)  +  (y_left*yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_left)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

thetal_=  atan(y_left/x_left)  -  atan(k2_left/kl_left); 


133 


dcs5->setRot(0  ,(57.3  *  theta  1),  0); 
dcs4->setRot(0  ,(57.3  *  theta2),  0); 

//  Right  arm 

dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  Torso  rotation 
if((z-  42  *(i- 1))<31){ 
dcs3->setRot((-(z-  42  *(i  -  1)  -  21)/3.0),0  ,0); 
dcs6->setRot((-(z-  42  *(i  -  1)  -21)/3.0),0  ,0); 

} 

else{ 

dcs3->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0),0  ,0); 
dcs6->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0),0  ,0); 

} 


dcsO->setTrans(X  +  1 .0  +  z  *  0. 1  ,  Y  ,  Z  +  0.2  +  0.08f 
*  cos(2.0f  *  3. 141 59f  *  z  /  21  .Of)); 
UpdateViewQ; 

UpdateGUIO; 

pfFrameO; 

} 

}// Big  FOR 

//  LAST  HALF  STEP ///////// 

for  (  z  =  1  ;  z  <  12  ;  z  +=  0.5  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 

Shared->simTime=pfGetTime(); 

/*  Main  Body  */ 

dcs0->setRot(90.0,  90.0 , 0); 
dcsO~>setTrans(X,Y,Z), 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->$etScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 


/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
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dcs8~>setTrans(0  ,-1.7,0); 


/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7,  0); 

/*  Right  lower  arm  */ 
dcs!2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2  ,  -4.6,  0.0); 

IIIIIIIIIIIIIIIIllllllllllllllIIIIIIIIIIIIIIIIIIIHIIIII 
//  Inverse  Kinematics 

llllllllllllillllllllllllllllllllllllllllllllUIIIHIUI 

li  LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  z)  *  DEG_TO_RAD  )  *(11  + 12) 

*  cos((22  -  2*  z)  *  DEGJTOJRAD); 

yjeft  =  0.95f  *  cos((22  -  2*  z)  *  DEG_TO_RAD  )  *(11  +  12) 

*  sin((22  -  2*  z)  *  DEG_TO_RAD); 

cl_left=  ((xjeft*  x_left)  +  (yjeft*  yjeft) 

-  (11*11)  -  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 

k2Jeft  =  12  *  sin(theta2); 

theta  1  =  atan(y_left/x_left)  -  atan(k2Jeft/kl  Jeft); 

dcs5->setRot(0  ,(57.3  *  theta  1),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 
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//  Right  arm 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  lO.Of), 0 ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  RIGHT  LEG 

x_right  =  cos((-22  +  2  *  z)  *  DEG_TO_RAD  )  *  (11  +12) 

*  cos((-22  +  2  *  z)  *  DEG_TO_RAD); 

y_right  =  cos((-22  +  2  *  z)  *  DEG_TO_RAD  )  *  (11  +12) 

*  sin((-22  +  2  *  z)  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (yjright*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl  fright)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  TORSO  rotation 
if(z  <  1 1 ){ 

dcs3->setRot((z/3.0),0  ,0); 
dcs6->setRot((z/3.0),0  ,0); 

} 

else{ 

dcs3->setRot(((20  -  z)/3.0),0  ,0); 
dcs6->setRot(((20  -  z)/3.0),0  ,0); 

} 

dcsO->setTrans(X  +  1.0  +  (i-1)  *  4.2  +  z  *  0.1 

,  Y  ,  +Z  +  0.2  +  0.08f  *cos(2.0f  *  3.14159f  *  z  /  21  .Of)); 

UpdateViewO; 

UpdateGUIO; 

pfFrame(); 

}//  END  of  step_forward 


// - 

//  Function: 

//  Returns: 

//  Parameters: 
//  Summary: 

// 

II 

// 

II 

II 

// - 


step_upward 

None 

Number  of  steps  needs  to  be  taken,  initial  position 
Computes  the  joint  angles  according  to  the  position 
of  the  end  effector(foot)  by  using  the  inverse  kinematic 
equations  of  the  three  link  planar  manipulator.  The 
algorithm  for  the  path  of  the  foot  is  described  in 
Chapter  IV  of  this  thesis  as  “Stepping  Upward  Algorithm. 
Height  of  each  step  is  0.267949  units. 


void  step_upward(int  number_of_steps, float  X, float  Y, float  Z){ 


float  11  =  l.Of; 
float  12  =  l.Of; 
float  cl_left,cl_right; 
float  theta l,theta2; 
float  kljeft,k2_left; 
float  kl_right,k2_right; 

float  xjeft  =  1 .732f; 
float  yjeft  =  O.Of; 
float  x_right  =  1.732f; 
float  y_right  =  O.Of; 
float  z; 

IIIIIIIIIIIHIIIIIIIIIIIIIIIIIIIIIIII 

ll  FIRST  HALF  STEP 

llllllllllinilllllllllllllllllllll 

for  (  z  =  1 1 .0  ;  z  <  22.0  ;  z  +=  0.3  *  delta„t) 

{ 


/*  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 

Shared->simTime=pfGetTime(); 

/*  Main  Body  *1 
dcs0->setRot(90.0,  90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  *1 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f) ; 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 


/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
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dcs9->setTrans(0  ,-1.7,0); 


/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,  -1.7,  0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 

//dcsl4->setTrans(1.6  ,1.8,0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

illlllllllllllllllllllilililllllllllllllllllllllilllllll 

//  Inverse  Kinematics 

llllllllllllllllIlllllllllllllillllilllllllllllHIIIHH 

if  FIRST  STEP 
//  LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD  )  *(11  +  12) 

*  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD); 

yjeft  =  0.95f  *  cos((22  -  2*  z  -  10)  *  DEG_TO_RAD  )  *(11  + 12) 

*  sin((22  -  2*  z  -  10)  *  DEG_TO_RAD); 

cl_left=  ((x_left* xjeft)  +  (yjeft*yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 

theta  1  =  atan(yjeft/xjeft)  -  atan(k2Jeft/kl  Jeft); 
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dcs5->setRot(0  ,(57.3  *  thetal),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 

H  Right  arm 

dcsll->setTrans(-1.0 , 1.7,  0.5); 
dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  lO.Of), 0  ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  RIGHT  LEG 

x. right  =  cos((-22  +  2  *  z  - 10)  *  DEG_TO_RAD  )  *  (11  + 12) 

*  cos((-22  +  2  *  z  -  10)  *  DEG_TO_RAD); 

y. right  =  cos((-22  +  2  *  z  -  10)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  sin((-22  +  2  *  z  -  10)  *  DEG_TO_RAD); 
cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/kl _right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,1.8, 0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0  ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  TORSO  rotation 

if(z  <  11){ 

dcs3->setTrans(0.3  ,  2.1 , 0.305); 
dcs6->setTrans(0.3  ,2.1,0.305); 
dcs3->setRot((z/3.0), 7.0,0); 
dcs6->setRot((z/3.0),7.0,0); 

} 

else{ 

dcs3->setTrans(0.3  ,2.1, 0.305); 
dcs6->setTrans(0.3  ,  2.1 , 0.305); 
dcs3->setRot(((20  -  z)/3.0)  ,7.0,0); 
dcs6->setRot(((20  -  z)/3.0)  ,7.0,0); 

} 

z  =  z+21; 

dcsO->setTrans(  X  +  (z  -  32)  *  0. 1  -0.4,  Y  , 

Z-  2.005063  +/*  0.08f  *cos(2.0f  *  3.14159f  *  z  /  21. Of)  */0 
+  1  *  0.267949  *  3.4  *2  +  ((11  +  12) 

-(11  +  12)  *  cos((  -22  +  2  *  z  -  10)  *  DEG_TO_R AD))) ; 
z  =  z-21; 


UpdateView(); 

UpdateGUIO; 

pfFrame(); 
}//END  FOR 
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for(  i  =  1  ;  i  <  number_of_steps  ;  i++)  { 

for  ( z  =  (i-l)*42  +  22.0  ;  z  <  (i-l)*42  +  43.0  ;  z  +=  0.35  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncQ; 

Shared->simTime  =  pfGetTime(); 

/*  Main  Body  */ 
dcs0->setRot(90.0,  90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 

dcs3->setTrans(0.3  ,2.1,0.005); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,  -1.7,  0); 

/*  Right  foot  */ 
dcs8->setScale(0. 7f) ; 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,  -1.7,  0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcs!3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcs  1 4->setScale(0.25f); 

/*  Left  lower  arm  */ 
dcsl5-->setRot(0,  0 , 180.0); 
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dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 


/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

lllllllllllllllllillllllllllllllllllllllllflllilllllllll 
I  I  Inverse  Kinematics 

IIIIIllllIlllllllIllllIIllllllllllilllllllllHIIIHIIIII 


llllllllllllllllllllllllllflllllllllllllllllllllllllllllllll 

I  I  SECOND  STEP 
//  RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD) 

*  (11  + 12)  *  cos((64  -  2*  (z-  42  *(i  - 1))  -  10) 

*  DEG_TO_RAD); 

y_right  =  0.95f  *  cos((64  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG„TO_RAD) 

*  (11  + 12)  *  sin((64  -  2*  (z-  42  *(i  -  1))  -  10) 

*  DEG_TOJRAD); 

cl_right=  ((x_right*x_right)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2„right  =  12  *  sin(theta2); 

thetal  =  atan(y_right/x_right)  -  atan(k2_right/k  1  _right) ; 

//  Right  LEG 

dcs2->setRot(0  ,(57.3  *  thetal  ),0  ); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,  1.8,  0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 


//  LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO„RAD  ) 
*  (11  + 12) 

*  cos((-64  +  2*  (z-  42  *(i  -  1))  - 10)  *  DEG_TO_RAD); 
yjeft  =  cos((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEGJTOJtAD  ) 

*  (11  +12) 

*  sin((-64  +  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD); 

cl_left=  ((xjeft*xjeft)  +  (y_left*y_left)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (1  *acos(cl_left)); 

kljeft  =  11  +  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 

thetal  =  atan(yjeft/x_left)  -  atan(k2_left/kl  Jeft); 
dcs5->setRot(0  ,(57.3  *  thetal),  0); 
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dcs4->setRot(0  ,(57.3  *  theta2),  0); 


//  Right  arm 

dcsll->setTrans(-1.0 , 1.7,  0.5); 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  TORSO  rotation 
if((z-  42  *(i  -  1))  <  31){ 
dcs3->setTrans(0.3  ,  2.1 , 0.305); 
dcs6->setTrans(0.3  ,  2.1 , 0.305); 
dcs3->setRot((-(z-  42  *(i  -  1)  -  21)/3.0)  ,7.0,0); 
dcs6->setRot((-(z-  42  *(i  -  1)  -21)/3.0)  ,7.0,0); 

} 

else{ 

dcs3->setTrans(0.3  ,  2.1 , 0.305); 
dcs6->setTrans(0.3  ,  2.1 , 0.305); 
dcs3->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0)  ,7.0,0); 
dcs6->setRot(((-20  +  ((z-  42  *(i  -  1)-  21)))/3.0)  ,7.0,0); 


z  =  z-21; 

dcs0->setTrans(  X  +  0.5  +  z  *  0.1  ,  Y  ,  Z 
+  i  *  0.267949  *  3.4  *2  -  ((11  + 12)  -  (11  +  12) 

*  cos((  64  -  2  *  (z-  42  *(i  -  1)) )  *  DEG_TO_RA D))); 
z  =  z+21; 

Update  View(); 

UpdateGUIO; 

pfFrame(); 


for  ( z  =  (i-l)*42  +  1.0  ;  z  <  (i-l)*42  +  22.0  ;  z  +=  0.35  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 

Shared->simTime=pfGetTime(); 

f*  Main  Body  */ 

dcs0->setRot(90.0,  90.0  ,0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7*->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 
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/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,  -1.7,  0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,  -1.7,  0); 

/*  Right  upper  arm  */ 
dcs  1 1  ->setScale(0.25f); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 

dcsl4->setScale(0.25f); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

iiiiiiiiiiiiiiiiiiiiiiiiniiiiiiiiiiiiiiiiiiiiiiiiniiii 

I  I  Inverse  Kinematics 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIUIHIIIM 

I  I  FIRST  STEP 
//  LEFT  LEG 

xjeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 
*(11  + 12)  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *DEG_TO_RAD); 
y Jeft  =  0.95f  *  cos((22  -  2*  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 
*(11  + 12)  *  sin((22  -  2*  (z-  42  *(i  -  1))  -  10)  *DEG_TO_RAD) ; 
cl_left=  ((x_left*x_left)  +  (y _lef t * y _1  eft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2Jeft  =  12  *  sin(theta2); 
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theta  1  =  atan(y_left/x_left)  -  atan(k2_Ieft/kl  Jeft); 

dcs5->setRot(0  ,(57.3  *  thetal),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 

//  Right  arm 

dcsll->setTrans(-1.0 , 1.7,  0.5); 
dcsll->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0  ); 
dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  RIGHT  LEG 

x_right  =  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*  01  +12)  *  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10) 

*  DEG_TO_RAD); 

y_right  =  cos((-22  +  2  *  (z-  42  *(i  -  1))  -  10)  *  DEG_TO_RAD  ) 

*  (11  + 12)  *  sin((-22  +  2  *  (z-  42  *(i  - 1))  - 10) 

*  DEG_TO_RAD); 

cl_right=  ((x_right*xjright)  +  (y_right*y_right)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

thetal  =  atan(y„right/x_right)  -  atan(k2_right/kl_right); 

dcs2->setRot(0  ,(57.3  *  thetal),  0); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,1.8, 0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  TORSO  rotation 
if((z-  42  *(i  - 1))  <  1 1){ 
dcs3->setTrans(0.3  ,  2.1  ,  0.305); 
dcs6->setTrans(0.3  ,  2.1 , 0.305); 
dcs3->setRot(((z-  42  *(i  -  l))/3.0)  ,7.0  ,0); 
dcs6->setRot(((z-  42  *(i  -  l))/3.0)  ,7.0  ,0); 

} 

else{ 

dcs3->setTrans(0.3  ,  2.1  ,  0.305); 
dcs6->setTrans(0.3  ,2.1,0.305); 
dcs3->setRot(((20  -  (z-  42  *(i  -  l)))/3.0)  ,7.0  ,0); 
dcs6->setRot(((20  -  (z-  42  *(i  -  l)))/3.0)  ,7.0  ,0); 

} 

z  =  z+21; 

dcsO->setTrans(X  +  0.5  +  z  *  0.1  ,  Y  ,  Z  -0.105327 

+  i  *  0.267949  *  3.4  *2  +  ((11  +  12)  -(11  +  12) 

*  cos((  -22  +  2  *  (z-  42  *(i  -  1))  -  10) 

*  DEGJXLRAD))); 

z  =  z-21; 

UpdateView(); 

UpdateGUIO; 

pfFrame(); 

}//END  FOR 
}//Big  FOR 
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IlillllllillllllllllllllllllllllllHIHIHIIlllHHII 
1/  LAST  HALF 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIHHIIHIIHI 

for  ( z  =  22.0  ;  z  <  33.0  ;  z  +=  0.35  *  delta_t){ 
/*  Go  to  sleep  until  next  frame  time.  */ 
pfSync(); 

Shared->simTime  =  pfGetTime(); 

/*  Main  Body  */ 
dcs0->setRot(90.0,  90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/*  Head  */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1, 0.005); 


/*  Left  foot  */ 
dcs9->setScale(0.7f) ; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  *1 
dcsl->setTrans(0  ,  -1.7, 0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcs  14->setScale(0.25f); 


/*  Left  lower  arm  */ 
dcsl5->setRot(0,  0 , 180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

lllllllllllllllllllillllllllllllllllllllllilllllltllllll 

//  Inverse  Kinematics 

llllllllllllillllllllllllllllilllllllllllllHIUHinHI 


llllltllllllllllllllllllllllllllllllllllllllllHHIIllH 

If  SECOND  STEP 
//  RIGHT  LEG  STEP 

x_right  =  0.95f  *  cos((64  -  2*  z  - 10)  *  DEGJTOJRAD  )*  (11  +  12) 

*  cos((64  -  2*  z  -  10)  *  DEGJTOJRAD); 

y_right  =  0.95f  *  cos((64  -  2*  z  -  10)  *  DEGJTOJRAD  )*  (11  +  12) 

*  sin((64  -  2*  z  -  10)  *  DEGJTOJRAD); 

cl_right=  ((x_ri ght* x_ri ght)  +  (y_right*y_right)  -  (11*11) 

-  (12*12) )/(2  *  11  *12); 
theta2  =  (l  *acos(cl_right)); 

kl_right  =  11  +  (12  *  cos(theta2)); 
k2_right  =  12  *  sin(theta2); 

theta  1  =  atan(y_right/x_right)  -  atan(k2_right/kl_right); 

//  Right  LEG 

dcs2->setRot(0  ,(57.3  *  theta  1),0  ); 
dcsl->setRot(0  ,(57.3  *  theta2),  0); 

//  Left  arm 

dcsl4->setTrans(1.6  ,  1.8,  0.5); 
dcsl4->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.00,0 ); 
dcsl5->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  10.00,  0); 

//  LEFT  LEG  STEP 

xjeft  =  cos((-64  +  2*  z  -  10)  *  DEG_TO_RAD  )  *  (11  +  12) 

*  cos((-64  +  2*  z  -  10)  *  DEG_TO_R AD) ; 

yjeft  =  cos((-64  +  2*  z  -  10)  *  DEG_TO_RAD  )  *  (11  + 12) 

*  sin((-64  +  2*  z  -  10)  *  DEGJTOJRAD); 

cl  Jeft=  ((xjeft* xjeft)  +  (yjeft* yjeft)  -  (11*11) 

-  (12*12) )/(2  *  11  *12); 
theta2  =  (1  *acos(cl  Jeft)); 


kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2Jeft  =  12  *  sin(theta2); 

thetal  =  atan(yjeft/xjeft)  -  atan(k2Jeft/kl  Jeft); 
dcs5->setRot(0  ,(57.3  *  thetal),  0); 
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dcs4->setRot(0  ,(57.3  *  theta2),  0); 
//  Right  arm 


dcsll->setTrans(-1.0 , 1.7,  0.5); 

dcsl  l->setRot(0  ,(0.7f  *(57.3  *  thetal)  +  10.0f),0 ); 

dcsl2->setRot(0  ,(1.5f  *(57.3  *  thetal)  -  lO.Of),  0); 

//  Torso  rotation 
if(z  <  31){ 

dcs3->setTrans(0.3  ,  2.1 , 0.305); 
dcs6->setTrans(0.3  ,2.1,0.305); 
dcs3->setRot((-z  -  21)/3.0  +13.75,7.0,0); 
dcs6->setRot((-z  -21)/3.0  +13.75,7.0,0); 

} 

else{ 

dcs3->setTrans(0.3  ,2.1,0.305); 
dcs6->setTrans(0.3  ,2.1,0.305); 
dcs3->setRot((-20+  z-  21)/3.0  ,7.0,0); 
dcs6->setRot((-20  +  z-  21)/3.0  ,7.0,0); 


z  =  z-21; 

dcsO->setTrans(  X  +  0.5  +(i-l)  *  4.2+  z  *  0.1  ,  Y  ,  Z 

+  number_of_steps  *  0.267949  *  3.4  *2  -  ((11  +  12) 
-  (11  + 12)  *  cos((  64  -  2  *  z  )  *  DEG_TO„RAD))); 


z  =  z+21; 

Update  Vie  w(); 

UpdateGUIO; 

pfFrame(); 

}//End  of  for 
}//End  of  step_upward 
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// - 

//  Function:  jump 

//  Returns:  None 

//  Parameters:  Initial  position 

//  Summary:  Translate  the  whole  body  first  straight  upward, 

//  secondly  along  a  semi  circle  path,  then  straight  down 

//  and  straight  up  to  an  upright  position.  While  translatin 

//  the  body  appropriate  joint  angles  are  applied. 

// - 

void 

jump(float  X,fIoat  Y,float  Z){ 

float  11  =  l.Of; 
float  12=  l.Of; 
float  cl  Jeft, cl  fright; 
float  theta l,theta2; 
float  kl_left,k2_left; 
float  kl_right,k2_right; 

float  x_left=  1.732f; 
float  yjeft=  O.Of; 
float  x_right  =  1 .732f; 
float  y_right=  O.Of; 

for  (float  j  =  1  ;  j  <  101  ;  j  +=  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncO; 

Shared->simTime  =  pfGetTime(); 

/*  Main  Body  */ 

dcs0->setRot(90.0,  90.0 , 0); 

dcsO->setTrans(X+j  *0.8/1 00. 0, Y ,Z  -  j  *  1.5/100.0); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f) ; 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 
dcs3->setRot(0,  10.0*j/100.0 , 0.0); 
dcs6~>setRot(0,  10.0*j/100.0 , 0.0); 

x_left  =  1.95-  1.0  *j/l  00.0; 
y_left  =  0.0; 

cl  Jeft=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl_left)); 

kl  Jeft  =  11  4-  (12  *  cos(theta2)); 
k2_left  =  12  *  sin(theta2); 


theta  1  =  atan(y_left/x_left)  -  atan(k2 Jeft/k  1  _left) ; 

dcs5->setRot(0  ,(57.3  *  thetal),0  ); 
dcs4->setRot(0  ,(57.3  *  theta2) ,  0); 
dcs2->setRot(0  ,(57.3  *  thetal),0 ); 
dcsl->setRot(0  ,(57.3  *  theta2) ,  0); 

/*  Left  foot  */ 
dcs9->setScale(0.7f) ; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,  -1.7,  0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,  -1.7, 0); 

/*  Right  upper  arm  */ 
dcs  1 1  ->setScale(0.25f); 
dcsll->setTrans(-1.0  ,  1.7,  0); 
dcsll->setRot(0.0,-j/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-j/5. 0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0  ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-j/5. 0,0.0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,-j/5.0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

I*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrame(); 
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for  (float  k  =  1  ;  k  <  101  ;  k  +=  5.0  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncO; 

Shared->simTime  =  pfGetTime(); 

xjeft  =  0.95  +  0.5*  k /1 00.0; 
y_left  =  0.0; 

/*  Main  Body  */ 

dcs0->setRot(90.0 , 90.0  +  15.0*k/100.0 , 0); 
dcsO->setTrans(X+  100.0*0.8/100.0  +  (xjeft*  1.7 
*  sin(15.0*k*DEG_TO_RAD/100.0)),  Y  , 

Z-  100.0  *  1.5/100.0  +  0.95  *  k/100.0  -  xjeft*  1.7 
*(1.0  -  cos(  1 5 .0*k*DEG  JTO  JRAD/100.0))); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dc$6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0,  10.0 , 0.0); 

cl  Jeft=  ((xjeft* xjeft)  +  (yjeft*yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2Jeft  =  12  *  sin(theta2); 

theta  1  =  atan(yjeft/xjeft)  -  atan(k2 Jeft/k  1  Jeft); 

dcs5->setRot(0  ,(57.3  *  theta  1)  ,0); 
dcs4->setRot(0  ,(57.3  *  theta2)  ,0); 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0); 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 
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/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 


/*  Right  upper  arm  */ 
dcs  1 1  ->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7,  0); 
dcsll->setRot(0.0,-j/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-j/5. 0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-j/5.0,0.0); 

/*  Left  lower  arm  */ 
dcsl5->setRot(0,-j/5.0,  180.0); 
dcsl5->setTrans(-0.5  ,  -4.6, 0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6, 0.0); 

UpdateView(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (float  1=1  ;  1  <  101  ;  1  +=  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncQ; 

Shared->simTime  =  pfGetTime(); 

xjeft  =1.45; 
yjeft  =  0.0; 

/*  Main  Body  */ 

dcs0->setRot(90.0 , 90.0+  360.0*1/100.0 , 0); 

dcs0->setTrans(3.0  +  3.0*cos((180.0  -  180.0*  1/100.0)*DEG_TO_RAD)+  X 
+  100.0*0.8/100.0  +  (xjeft*  1.7 

*  sin(  1 5 .0*  1 00.0*DEG_TO_RAD/ 1 00.0)), Y  , 

3.0*sin((180.0  -  180.0*  1/100.0)*DEG_TO_RAD) 

+  Z-  100.0  *  1.5/100.0+  0.95  *  100.0/100.0  -  xjeft 

*  1.7*(1.0  -  cos(  1 5 . 0*  1 00.0* DEG_TO_R AD/ 100.0))); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 
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/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1  ,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0,  10.0 , 0.0); 

cl_left=  ((x_left*x_left)  +  (y_left*y_left)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl_left)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 

k2_left  - 12  *  sin(theta2); 

theta  1  =  atan(y_left/x_left)  -  atan(k2_left/kl  Jeft); 

dcs5->setRot(0  ,(57.3  *  theta  1)  ,0); 
dcs4->setRot(0  ,(57.3  *  theta2)  ,0); 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0); 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0); 

/*  Left  foot  */ 
dcs9->$etScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7,  0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcs  1 2->setRot(0.0  ,- 1 00.0/5 .0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 


/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcs  14->setRot(0.0  1 00.0/5.0,0.0); 

/*  Left  lower  arm  */ 

dcs  1 5->setRot(0,-  1 00.0/5.0,  1 80.0); 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

Update  View(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (float  m  =  1  ;  m  <  101  ;  m  +=  3.0  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncQ; 

Shared->simTime  =  pfGetTime(); 

xjeft  =1.45; 
yjeft  =  0.0; 

/*  Main  Body  */ 

dcs0->setRot(90.0 , 90.0+  360.0*100.0/100.0 , 0); 

dcs0->setTrans(3.0  +  3.0*cos((180.0  -  180.0*  1 00.0/1 00.0)*DEG_TO_RAD)+X 
+  100.0*0.8/100.0  +  (xjeft*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  , 

3.0*sin((180.0  -  180.0*  1 00.0/ 1 00.0)*  DEG  JTO  JR  AD) 

+  Z-  100.0  *  1.5/100.0  +  0.95  *  100.0/100.0  -  xjeft 

*  1.7* (1.0  -  cos(15.0*100.0*DEGJTOJRAD/100.0)) 

-  9.01  *m/l 00.0); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,  2.1  ,  -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0,  10.0 , 0.0); 

cl  Jeft=  ((xjeft* xjeft)  +  (yjeft* yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 
k2Jeft  =  12  *  sin(theta2); 

theta  1  =  atan(yjeft/x_left)  -  atan(k2_left/kl  Jeft); 
dcs5->5etRot(0  ,(57.3  *  thetal)  ,0); 
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dcs4->setRot(0  ,(57.3  *  theta2)  ,0); 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0); 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

1*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,-1.7,0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsl  l->setTrans(-1.0 , 1.7,  0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4  ,  -4.6,  0.0); 
dcsl 2->setRot(0.0  ,- 1 00.0/5 .0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 ,  180.0); 

/*  Left  upper  arm  */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6  ,1.8,0); 
dcs!4->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Left  lower  arm  */ 

dcsl  5->setRot(0,- 1 00.0/5.0,  1 80.0) ; 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

Update  View(); 

UpdateGUIO; 

pfFrame(); 

} 

for  (float  n  =  1  ;  n  <  101  ;  n  +=  5.0  *  delta_t){ 
/♦  Go  to  sleep  until  next  frame  time.  */ 

pfSyncO; 


Shared->simTime  =  pfGetTime(); 

x_left=  1.45  +  0.5  *n/100.0; 
y  Jeft  =  0.0; 

/*  Main  Body  */ 

dcs0->setRot(90.0 , 90.0+  360.0*100.0/100.0 , 0); 

dcs0->setTrans(3.0  +  3.0*cos((180.0  -  180.0*  1 00.0/1 00.0)*DEG„TO_RAD)+X 
+  100.0*0.8/100.0 +  (1.45*  1.7 

*  sin(15.0*100.0*DEG_TO_RAD/100.0)),Y  , 

3.0*sin((180.0-  180.0*  100.0/100.0)*DEG_TO_RAD) 

+  Z-  100.0*  1.5/100.0  +  0.95  *  100.0/100.0-  1.45 

*  1.7*(1.0  -  cos(15.0*100.0*DEG_TO_RAD/100.0)) 
-9.01*100.0/100.0+  n*  1.8/100.0); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 

/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3  ,  2.1 , 0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0,  10.0 , 0.0); 

cl_left=  ((x  Jeft*  x  Jeft)  +  (yjeft*yjeft)  -  (11*11) 

-  (12*12))/(2  *  11  *12); 

theta2  =  (1  *acos(cl  Jeft)); 

kl_left  =  11  +  (12  *  cos(theta2)); 

k2Jeft  =  12  *  sin(theta2); 

theta  1  =  atan(yjeft/x_ieft)  -  atan(k2Jeft/kl  Jeft); 

dcs5->setRot(0  ,(57.3  *  thetal)  ,0); 
dcs4->setRot(0  ,(57.3  *  theta2)  ,0); 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0); 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,-1.7,0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,  -1.7,  0); 

/*  Left-upperleg  */ 
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dcs5->setTrans(0.6,  0,  0); 


/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,  -1.7,  0); 

/*  Right  upper  arm  */ 
dcsl  l->setScale(0.25f); 
dcsll->setTrans(-1.0 ,  L7, 0); 
dcsl  1  ->setRot(0.0,- 1 00.0/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcsl2->setTrans(0.4 ,  -4.6,  0.0); 
dcsl2->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcsl3->setRot(0,  0 , 180.0); 

/*  Left  upper  arm  */ 
dcs  1 4->setScale(0. 25f) ; 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Left  lower  arm  */ 

dcs  1 5->setRot(0,- 1 00.0/5.0,  1 80.0); 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(-0.2 ,  -4.6,  0.0); 

UpdateView(); 

UpdatcGUIO; 

pfFrame(); 

} 

for  (float  o  =  l  ;  o  <  101  ;  o  +=  2.0  *  delta_t){ 

/*  Go  to  sleep  until  next  frame  time.  */ 
pfSyncQ; 

Shared->simTime  =  pfGetTime(); 

xjeft  =  1.45  +  0.5  *100.0/100.0; 
y_left  =  0.0; 

/*  Main  Body  */ 

dcs0->setRot(90.0 , 90.0+  360.0*100.0/100.0 , 0); 

dcsO->setTrans(3 .0  +  3.0*cos((180.0  -  180.0*  100.0/100.0)*DEG_TO_RAD)+X 
+  100.0*0.8/100.0 +  (1.45*  1.7 

*  sin(l 5.0*  100.0* DEG_TO_RAD/l 00.0)), Y  ,  3.0 
*sin((180.0-  180.0*  100.0/100.0)*DEG_TO_RAD) 

+  Z-  100.0  *  1.5/100.0  +  0.95  *  100.0/100.0  -  1.45*  1.7 

*  (1.0  -  cos(l  5.0*  100.0*DEGJTO_RAD/1 00.0)) 

-9.01*100.0/100.0  +  n*  1.8/100.0); 

/*  Head  */ 

dcs7->setTrans(0.0  ,  0.4 , 0.0); 
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/*  Torso  */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3  ,2.1,-0.005); 
dcs3->setScale(2.0f) ; 
dcs3->setTrans(0.3  ,2.1,0.005); 
dcs3->setRot(0,  10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl  Jeft=  ((xjeft*x_left)  +  (yjeft*yjeft)  -  (11*11) 
-  (12*12))/(2  *  11  *12); 

theta2  =  (l  *acos(cl  Jeft)); 

kl  Jeft  =  11  +  (12  *  cos(theta2)); 

k2_left  =  12  *  sin(theta2); 

theta  1  =  atan(y_left/x_left)  -  atan(k2_left/kl  Jeft); 

dcs5->setRot(0  ,(57.3  *  thetal)  ,0); 
dcs4->setRot(0  ,(57.3  *  theta2)  ,0); 
dcs2->setRot(0  ,(57.3  *  thetal)  ,0); 
dcsl->setRot(0  ,(57.3  *  theta2)  ,0); 

/*  Left  foot  */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0,  0 , 180.0); 
dcs9->setTrans(0  ,  -1.7,  0); 

/*  Right  foot  */ 
dcs8->setScale(0.7f) ; 
dcs8->setRot(0,  0 , 180.0); 
dcs8->setTrans(0  ,-1.7,0); 

/*  Left  lowerleg  */ 
dcs4->setTrans(0  ,-1.7,0); 

/*  Left  upperleg  */ 
dcs5->setTrans(0.6,  0,  0); 

/*  Right  lowerleg  */ 
dcsl->setTrans(0  ,  -1.7,  0); 

/*  Right  upper  arm  */ 
dcsll->setScale(0.25f); 
dcsll->setTrans(-1.0 , 1.7,  0); 
dcsl  l->setRot(0.0,- 100.0/5.0  ,0.0); 

/*  Right  lower  arm  */ 
dcs!2->setTrans(0.4 ,  -4.6,  0.0); 
dcs  1 2->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Right  hand  */ 
dcsl3->setTrans(0.0 ,  -4.2,  0.0); 
dcs!3->setRot(0,  0 , 180.0); 

/*  Left-upper  arm  */ 
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dcs  1 4->setScale(0.25f) ; 
dcsl4->setTrans(1.6  ,1.8,0); 
dcsl4->setRot(0.0  ,-100.0/5.0,0.0); 

/*  Left  lower  arm  */ 

dcs  1 5->setRot(0,-l  00.0/5.0,  1 80.0) ; 

dcsl5->setTrans(-0.5  ,  -4.6,  0.0); 

/*  Left  hand  */ 
dcsl6->setTrans(~0.2 ,  -4.6,  0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrameQ; 
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